﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;

//EMGU
using Emgu.CV;
using Emgu.CV.Structure;
using Emgu.Util;
using Emgu.CV.Util;
using DirectShowLib;

using System.Threading;
using System.Diagnostics;
using System.Runtime.InteropServices;
using System.Xml;
using System.IO.Ports;
using System.IO;

namespace VehicleTest
{
    public partial class Calibration : Form
    {
        #region Display and aquaring chess board info
        public MainWindows mainwindows_calibration;
        private Capture _Capture; // capture device
        private bool capture_flag = true;
        private bool capture_start = false;
        private Video_Device[] WebCams;

        private SerialPort LMS111_serial = new SerialPort();
        private bool serial_receive_on = false;
        //LMS111返回数据
        private int[] LMS_data_cali;
        public int LMS_NUM_cali = 241;
        public double LMS_Resolution = 0.5;
        public double LMS_StartAngle = 30;
        public double LMS_EndAngle = 150;
        private bool LMS111_open_cali;
        private byte[] cmd_single = { 0x02, 0x73, 0x52, 0x4E, 0x20, 0x4C, 0x4D, 0x44, 0x73, 0x63, 0x61, 0x6E, 0x64, 0x61, 0x74, 0x61, 0x03 };

        //绘制图像中的激光点,用于预览
        private bool flag_preview = false;
        private bool flag_previewfirst = true;
        private bool flag_lmsdata_done = false;
        private int[] LMS_u;     //激光点在图像上的x坐标
        private int[] LMS_v;     //激光点在图像上的y坐标
        private int[] LMS_u_temp;
        private int[] LMS_v_temp;
        private double[] LMS_X;     //激光点在水平平面XOZ'上的X坐标
        private double[] LMS_Z;     //激光点在水平平面XOZ'上的Z坐标
        private Matrix<double> intrinsic = new Matrix<double>(3, 3);
        private Matrix<double> distortion = new Matrix<double>(5, 1);
        private double alpha;
        private Matrix<double> rotation = new Matrix<double>(3, 3);
        private Matrix<double> rotation_AdjustOri = new Matrix<double>(3, 3); //调整旋转矩阵
        private Matrix<double> rotation_Adjust = new Matrix<double>(3, 3); //调整旋转矩阵
        private Matrix<double> deviation = new Matrix<double>(1, 3);
        private double[] Pf = new double[3] { 0, 0, 0 };
        private double[] P_transfer = new double[3] { 0, 0, 0 };

        //存储LMS111数据
        private bool file_init = false;
        private bool LMS111_dataReceived = false; //用于LMS111数据示意图显示
        private double[,] LMS_dataX_history;
        private double[,] LMS_dataY_history;
        private int[,] LMS_dataLimt_history;
        private FileStream file_LMS111rawdata; //存储LMS111原始数据
        private StreamWriter write_LMS111rawdata;
        private FileStream file_LMS111valuable; //存储LMS111有用数据
        private StreamWriter write_LMS111valuable;
        private FileStream file_EX_Param; //存储标定过程中每幅图像的外部参数
        private StreamWriter write_EX_Param;
        private FileStream file_IC_Param; //存储标定的内部参数
        private StreamWriter write_IC_Param;

        private Image<Bgr, Byte> img; // image captured
        private Image<Gray, Byte> Gray_Frame; // image for processing
        private int width = 9;//9 //width of chessboard no. squares in width - 1
        private int height = 6;//6 // heght of chess board no. squares in heigth - 1
        private Size patternSize; //size of chess board to be detected
        private PointF[] corners; //corners found from chessboard
        private Bgr[] line_colour_array; // just for displaying coloured lines of detected chessboard

        private static Image<Gray, Byte>[] Frame_array_buffer = new Image<Gray,byte>[100]; //number of images to calibrate camera over
        private int frame_buffer_savepoint = 0;
        private int frame_buffer_savepoint_pre = 0;
        private int interval = 100;
        //用于离线标定
        private int width_offline = 9; //离线数据
        private int height_offline = 6; //离线数据
        private int imageNum_offline = 20;
        private int imageCount_offline = 0;
        private Image<Bgr, Byte> img_offline;
        private Image<Gray, Byte> imgGray_offline;
        private PointF[] corners_offline; //corners found from chessboard
        private Size patternSize_offline; //size of chess board to be detected
        private Bgr[] line_colour_array_offline; // just for displaying coloured lines of detected chessboard
        private static Image<Gray, Byte>[] Frame_array_buffer_offline = new Image<Gray, byte>[100]; //number of images to calibrate camera over

        //用于标定激光测距仪倾角
        private double[] LMS_data_alpha1;  //用于存储倾角标定时第一组数据
        private double[] LMS_data_alpha1X; //用于存储倾角标定时第一组数据
        private double[] LMS_data_alpha1Y; //用于存储倾角标定时第一组数据
        private double[] LMS_data_alpha2;  //用于存储倾角标定时第一组数据
        private double[] LMS_data_alpha2X; //用于存储倾角标定时第二组数据
        private double[] LMS_data_alpha2Y; //用于存储倾角标定时第二组数据
        private bool flag_alpha1 = false;
        private bool flag_alpha2 = false;

        //数据保存到xml文件
        private XmlDocument xml_calibration = new XmlDocument();

        private bool start_Flag = false;
        private bool para_sure = false;
        private bool manual_flag = true;
        #endregion

        #region Current mode variables
        public enum Mode
        {
            Caluculating_Intrinsics,
            Calibrated,
            SavingFrames,
            Preview
        }
        Mode currentMode = Mode.SavingFrames;

        public enum LMSdataState
        {
            Preview,
            UnionCali,
            LMSCali,
            Idle
        }

        LMSdataState lmsState = LMSdataState.Idle;
        #endregion

        #region Getting the camera calibration
        MCvPoint3D32f[][] corners_object_list = new MCvPoint3D32f[Frame_array_buffer.Length][];
        PointF[][] corners_points_list = new PointF[Frame_array_buffer.Length][];
        MCvPoint3D32f[][] corners_object_list_offline = new MCvPoint3D32f[Frame_array_buffer_offline.Length][];
        PointF[][] corners_points_list_offline = new PointF[Frame_array_buffer_offline.Length][];

        IntrinsicCameraParameters IC = new IntrinsicCameraParameters();
        ExtrinsicCameraParameters[] EX_Param;

        #endregion

        public Calibration(MainWindows mw)
        {
            InitializeComponent();
 
            mainwindows_calibration = mw;
            CalibrationParametersLoad("Calibration.xml");
            AdjustRotationLoad();

            //禁用某些控件
            toolStripButton_Refresh.Enabled = false;
            toolStripButton_ReadCamera.Enabled = false;

            Start_BTN.Enabled = false;
            button_ParameterSure.Enabled = false;
            button_munal.Enabled = false;
            button_UndoMunal.Enabled = false;

            button_ParameterSureOffline.Enabled = false;
            button_OfflineCaliStart.Enabled = false;
            button_OfflineNext.Enabled = false;
            button_OfflineSure.Enabled = false;

            textBox_ScaleMin.Enabled = false;
            textBox_ScaleMax.Enabled = false;
            button_slecetdata.Enabled = false;
            numericUpDown_LMSIndex.Enabled = false;
            toolStripButton_CaliResult.Enabled = false;

            button_AlphaCaliStart.Enabled = false;
            button_Position1.Enabled = false;
            button_Position2.Enabled = false;
            button_AlphaParaSure.Enabled = false;
            button_AlphaCalculate.Enabled = false;
            textBox_MoveDistance.Enabled = false;
            textBox_AlphaScaleMin.Enabled = false;
            textBox_AlphaScaleMax.Enabled = false;
            button_Update.Enabled = false;

            //初始化LMS数据数组
            LMS_data_cali = new int[LMS_NUM_cali];
            LMS_data_alpha1 = new double[LMS_NUM_cali]; 
            LMS_data_alpha2 = new double[LMS_NUM_cali]; 
            LMS_data_alpha1X = new double[LMS_NUM_cali];
            LMS_data_alpha1Y = new double[LMS_NUM_cali]; 
            LMS_data_alpha2X = new double[LMS_NUM_cali];
            LMS_data_alpha2Y = new double[LMS_NUM_cali];
            LMS_u = new int[LMS_NUM_cali];
            LMS_v = new int[LMS_NUM_cali];
            LMS_u_temp = new int[LMS_NUM_cali];
            LMS_v_temp = new int[LMS_NUM_cali];
            LMS_X = new double[LMS_NUM_cali];
            LMS_Z = new double[LMS_NUM_cali];
            file_init = false;
            
            //读取摄像头
            DsDevice[] _SystemCamereas = DsDevice.GetDevicesOfCat(FilterCategory.VideoInputDevice);
            WebCams = new Video_Device[_SystemCamereas.Length];
            for (int i = 0; i < _SystemCamereas.Length; i++)
            {
                WebCams[i] = new Video_Device(i, _SystemCamereas[i].Name, _SystemCamereas[i].ClassID); //fill web cam array
                toolStripComboBox_CameraSelect.Items.Add(WebCams[i].ToString());
            }
            if (toolStripComboBox_CameraSelect.Items.Count > 0)
            {
                toolStripComboBox_CameraSelect.SelectedIndex = 0; //Set the selected device the default
                toolStripButton_ReadCamera.Enabled = true; //Enable the start
            }
            else
            {
                toolStripButton_ReadCamera.Enabled = false;
            }
            //读取串口
            foreach (string s in SerialPort.GetPortNames())
            {
                toolStripComboBox_LMS111Serial.Items.Add(s);
            }
            if (toolStripComboBox_LMS111Serial.Items.Count > 0)
            {
                toolStripComboBox_LMS111Serial.SelectedIndex = 0;
                toolStripButton_ReadLMS111.Enabled = true;
            }
            else
            {
                toolStripButton_ReadLMS111.Enabled = false;
            }
        }

        #region 控件事件
        private void toolStripButton_ReadCamera_Click(object sender, EventArgs e)
        {
            if ((_Capture == null) || (!capture_start))
            {
                try
                {
                    _Capture = new Capture(toolStripComboBox_CameraSelect.SelectedIndex);
                    _Capture.ImageGrabbed += new Emgu.CV.Capture.GrabEventHandler(_Capture_ImageGrabbed);
                }
                catch (NullReferenceException except)
                {
                    MessageBox.Show(except.Message);
                }
            }

            if (_Capture != null)
            {
                if (capture_flag)
                {
                    toolStripButton_ReadCamera.Text = "关闭摄像头";
                    toolStripComboBox_CameraSelect.Enabled = false;
                    toolStripButton_Refresh.Enabled = false;
                    button_ParameterSure.Enabled = true;
                    toolStripButton_OffLineCali.Enabled = false; //禁用离线标定
                    _Capture.Start();
                    richTextBox_Show.SelectionColor = Color.Green;
                    richTextBox_Show.AppendText("Camera is open!\n");
                    richTextBox_Show.ScrollToCaret();
                    capture_start = true;

                    if (LMS111_serial.IsOpen)
                        toolStripButton_CaliResult.Enabled = true;//使能预览
                }
                else
                {
                    toolStripButton_ReadCamera.Text = "打开摄像头";
                    toolStripComboBox_CameraSelect.Enabled = false;
                    toolStripButton_Refresh.Enabled = true;
                    button_ParameterSure.Enabled = false;
                    Start_BTN.Enabled = false;
                    capture_start = false;
                    _Capture.Stop();
                    _Capture.Dispose();
                    richTextBox_Show.SelectionColor = Color.Red;
                    richTextBox_Show.AppendText("Camera is closed!\n");
                    richTextBox_Show.ScrollToCaret();

                    para_sure = false;
                    start_Flag = false;
                    textBox_ImageDone.Text = "";
                    textBox_CalibrationState.Text = "";
                    radioButton_auto.Enabled = true;
                    radioButton_manual.Enabled = true;

                    toolStripButton_CaliResult.Enabled = true;//禁用预览
                    if (toolStripButton_ReadLMS111.Text == "打开LMS111")
                        toolStripButton_OffLineCali.Enabled = true; //启用离线标定
                }
                capture_flag = !capture_flag;
            }
        }

        private void toolStripButton_Refresh_Click(object sender, EventArgs e)
        {
            toolStripComboBox_CameraSelect.Enabled = true;
            _Capture.Dispose();
            _Capture = null;
            Main_Picturebox.Image = new Image<Bgr, Byte>(mainwindows_calibration.path + "/image/camerabackground2.jpg");
        }

        private void toolStripComboBox_CameraSelect_DropDown(object sender, EventArgs e)
        {
            //读取摄像头
            DsDevice[] _SystemCamereas = DsDevice.GetDevicesOfCat(FilterCategory.VideoInputDevice);

            toolStripComboBox_CameraSelect.Items.Clear();
            WebCams = new Video_Device[_SystemCamereas.Length];
            for (int i = 0; i < _SystemCamereas.Length; i++)
            {
                WebCams[i] = new Video_Device(i, _SystemCamereas[i].Name, _SystemCamereas[i].ClassID); //fill web cam array
                toolStripComboBox_CameraSelect.Items.Add(WebCams[i].ToString());
            }
        }

        private void toolStripComboBox_LMS111Serial_DropDown(object sender, EventArgs e)
        {
            toolStripComboBox_LMS111Serial.Items.Clear();
            foreach (string s in SerialPort.GetPortNames())
            {
                toolStripComboBox_LMS111Serial.Items.Add(s);
            }
        }

        private void toolStripButton_ReadLMS111_Click(object sender, EventArgs e)
        {
            try
            {
                if (LMS111_serial.IsOpen)
                {
                    serial_receive_on = false;
                    LMS111_open_cali = false;
                    LMS111_serial.Close();
                    LMS111_serial.DataReceived -= new SerialDataReceivedEventHandler(event_LMS111ReadSerialport);
                    richTextBox_Show.SelectionColor = Color.Red;
                    richTextBox_Show.AppendText("LMS111 have closed: " + toolStripComboBox_LMS111Serial.Text + "!\n");
                    richTextBox_Show.ScrollToCaret();
                    toolStripButton_ReadLMS111.Text = "打开LMS111";

                    if (toolStripButton_ReadCamera.Text == "打开摄像头")
                        toolStripButton_OffLineCali.Enabled = true; //启用离线标定

                    toolStripButton_CaliResult.Enabled = false;  //禁用预览
                    //禁用倾角标定
                    button_AlphaCaliStart.Enabled = false;
                    button_Position1.Enabled = false;
                    button_Position2.Enabled = false;
                    button_AlphaParaSure.Enabled = false;
                    button_AlphaCalculate.Enabled = false;
                    textBox_MoveDistance.Enabled = false;
                    textBox_AlphaScaleMin.Enabled = false;
                    textBox_AlphaScaleMax.Enabled = false;
                    button_Update.Enabled = false;
                }
                else
                {
                    LMS111_serial.PortName = toolStripComboBox_LMS111Serial.Text;
                    LMS111_serial.BaudRate = 115200;
                    LMS111_serial.DataBits = 8;
                    LMS111_serial.StopBits = StopBits.One;
                    LMS111_serial.Parity = Parity.None;

                    LMS111_serial.DataReceived += new SerialDataReceivedEventHandler(event_LMS111ReadSerialport);
                    LMS111_serial.Open();
                    serial_receive_on = true;
                    LMS111_open_cali = true;
                    richTextBox_Show.SelectionColor = Color.Green;
                    richTextBox_Show.AppendText("LMS111 have chosen: " + toolStripComboBox_LMS111Serial.Text + "!\n");
                    richTextBox_Show.ScrollToCaret();
                    toolStripButton_ReadLMS111.Text = "关闭LMS111";

                    toolStripButton_OffLineCali.Enabled = false; //禁用离线标定

                    button_AlphaCaliStart.Enabled = true; //使能倾角标定
                    button_Position1.Enabled = false;
                    button_Position2.Enabled = false;
                    button_AlphaParaSure.Enabled = false;
                    button_AlphaCalculate.Enabled = false;
                    textBox_MoveDistance.Enabled = false;
                    textBox_AlphaScaleMin.Enabled = false;
                    textBox_AlphaScaleMax.Enabled = false;
                    button_Update.Enabled = false;

                    if (toolStripButton_ReadCamera.Text == "关闭摄像头")
                        toolStripButton_CaliResult.Enabled = true; //使能预览

                    //记录数据
                    checkBox_HistoryData.Checked = false;
                    checkBox_HistoryData.Enabled = false;
                }
            }
            catch (System.UnauthorizedAccessException ex)
            {
                MessageBox.Show("串口冲突！重新选择可用串口:\n" + ex.Message);
            }
        }

        private void button_ParameterSure_Click(object sender, EventArgs e)
        {
            width = (int)Math.Round(numericUpDown_width.Value);
            height = (int)Math.Round(numericUpDown_height.Value);
            patternSize = new Size(width, height); //size of chess board to be detected
            line_colour_array = new Bgr[width * height]; // just for displaying coloured lines of detected chessboard

            if (radioButton_auto.Checked == true)
                interval = (int)Math.Round(numericUpDown_interval.Value);

            if (!file_init)
            {
                file_LMS111rawdata = new FileStream(mainwindows_calibration.path + "/doc/LMS111rawdata.txt", FileMode.Create);
                file_LMS111valuable = new FileStream(mainwindows_calibration.path + "/doc/LMS111data.txt", FileMode.Create);
                file_EX_Param = new FileStream(mainwindows_calibration.path + "/doc/LMS111_EX_Param.txt", FileMode.Create);
                file_IC_Param = new FileStream(mainwindows_calibration.path + "/doc/LMS111_IC_Param.txt", FileMode.Create);
                write_LMS111rawdata = new StreamWriter(file_LMS111rawdata);
                write_LMS111valuable = new StreamWriter(file_LMS111valuable);
                write_EX_Param = new StreamWriter(file_EX_Param);
                write_IC_Param = new StreamWriter(file_IC_Param);
                file_init = true;
            }

            //fill line colour array
            Random R = new Random();
            for (int i = 0; i < line_colour_array.Length; i++)
            {
                line_colour_array[i] = new Bgr(R.Next(0, 255), R.Next(0, 255), R.Next(0, 255));
            }

            Start_BTN.Enabled = true;
            button_ParameterSure.Enabled = false;
            if (radioButton_auto.Checked == true)
            {
                radioButton_auto.Enabled = false;
                radioButton_manual.Enabled = false;
            }
            if (currentMode != Mode.SavingFrames) currentMode = Mode.SavingFrames;
            para_sure = true;
        }

        private void button_ParameterSureOffline_Click(object sender, EventArgs e)
        {
            width_offline = (int)Math.Round(numericUpDown_widthOffline.Value);
            height_offline = (int)Math.Round(numericUpDown_heightOffline.Value);
            imageNum_offline = (int)Math.Round(numericUpDown_ImageNumOffline.Value);
            patternSize_offline = new Size(width_offline, height_offline);
            line_colour_array_offline = new Bgr[width_offline * height_offline];

            //fill line colour array
            Random R = new Random();
            for (int i = 0; i < line_colour_array_offline.Length; i++)
                line_colour_array_offline[i] = new Bgr(R.Next(0, 255), R.Next(0, 255), R.Next(0, 255));

            button_ParameterSureOffline.Enabled = false;
            button_OfflineCaliStart.Enabled = true;
        }

        private void Start_BTN_Click(object sender, EventArgs e)
        {
            Start_BTN.Enabled = false;
            //set up the arrays needed
            Frame_array_buffer = new Image<Gray, byte>[(int)numericUpDown_ImageNum.Value];
            corners_object_list = new MCvPoint3D32f[Frame_array_buffer.Length][];
            corners_points_list = new PointF[Frame_array_buffer.Length][];
            frame_buffer_savepoint = 0;
            //allow the start
            start_Flag = true;

            lmsState = LMSdataState.UnionCali;  //LMS接收数据处理状态变为联合标定
        }

        private void button_munal_Click(object sender, EventArgs e)
        {
            if (button_munal.Text == "计算")
            {
                write_LMS111rawdata.WriteLine("***");
                write_LMS111rawdata.WriteLine(numericUpDown_ImageNum.Value.ToString());
 
                currentMode = Mode.Caluculating_Intrinsics; //buffer full
                button_munal.Text = "手动";
            }
            else
            {
                manual_flag = true;
            }
            
        }

        private void radioButton_auto_CheckedChanged(object sender, EventArgs e)
        {
            if (radioButton_auto.Checked == false) //手动
            {
                button_munal.Enabled = true;
                button_UndoMunal.Enabled = true;
                numericUpDown_interval.Enabled = false;
                textBox_ScaleMin.Enabled = true;
                textBox_ScaleMax.Enabled = true;
                button_slecetdata.Enabled = true;

                interval = 0;
                manual_flag = false;
            }
            else  //自动
            {
                button_munal.Enabled = false;
                button_UndoMunal.Enabled = false;
                numericUpDown_interval.Enabled = true;
                textBox_ScaleMin.Enabled = false;
                textBox_ScaleMax.Enabled = false;
                button_slecetdata.Enabled = false;
                
                manual_flag = true;
            }
        }

        private void button_clear_Click(object sender, EventArgs e)
        {
            richTextBox_Show.Clear();
        }

        private void button_slecetdata_Click(object sender, EventArgs e)
        {
            //文件存储原始数据
            write_LMS111rawdata.WriteLine("*第" + frame_buffer_savepoint.ToString() + "幅图像*");
            for (int i = 0; i < LMS_NUM_cali; i++)
            {
                //存储在文件中的数据
                write_LMS111rawdata.Write("[" + i.ToString() + "] ");
                write_LMS111rawdata.Write(LMS_data_cali[i].ToString());
                write_LMS111rawdata.WriteLine(" mm");
            }

            int min = Convert.ToInt32(textBox_ScaleMin.Text);
            int max = Convert.ToInt32(textBox_ScaleMax.Text);
            write_LMS111valuable.WriteLine("*" + frame_buffer_savepoint.ToString() + "*");
            for (int i = min; i <= max; i++)
            {
                write_LMS111valuable.WriteLine(i.ToString() + " " + LMS_data_cali[i]);
            }
            write_LMS111valuable.WriteLine();
            MessageBox.Show("第"+ frame_buffer_savepoint.ToString() +"次数据截取成功！");
            button_munal.Enabled = true;  //为了保证每次激光数据都截取一次
        }

        private void textBox_ScaleMin_TextChanged(object sender, EventArgs e)
        {
            try
            {
                int min = Convert.ToInt32(textBox_ScaleMin.Text);
                int max = Convert.ToInt32(textBox_ScaleMax.Text);
                if ((min <= max) && (min >= 0))
                    button_slecetdata.Enabled = true;
                else
                    button_slecetdata.Enabled = false;
            }
            catch (FormatException)
            {
                button_slecetdata.Enabled = false;
            }
        }

        private void textBox_ScaleMax_TextChanged(object sender, EventArgs e)
        {
            try
            {
                int min = Convert.ToInt32(textBox_ScaleMin.Text);
                int max = Convert.ToInt32(textBox_ScaleMax.Text);
                if ((min <= max) && (max <= 240))
                    button_slecetdata.Enabled = true;
                else
                    button_slecetdata.Enabled = false;
            }
            catch (FormatException)
            {
                button_slecetdata.Enabled = false;
            }
        }

        private void toolStripButton_Calculate_Click(object sender, EventArgs e)
        {
            if (file_init)
            {
                write_LMS111rawdata.Close();
                write_LMS111valuable.Close();
                write_EX_Param.Close();
                write_IC_Param.Close();
            }
            Calculate_Ex_Para();

        }

        private void toolStripButton_Recover_Click(object sender, EventArgs e)
        {
            CalibrationParametersLoad("Calibration_Raw.xml");
        }

        private void button_OpenChart_Click(object sender, EventArgs e)
        {
            if (checkBox_HistoryData.Checked) //离线数据
            {
                try
                {
                    string[] line_LMSdata = File.ReadAllLines(mainwindows_calibration.path + "/doc/LMS111rawdata.txt");
                    string[] line_LMSdata_valid = File.ReadAllLines(mainwindows_calibration.path + "/doc/LMS111data.txt");
                    bool valid = false;
                    int NUM = 0; //图像个数
                    if (line_LMSdata.Length >= 2)
                    {
                        if (line_LMSdata[line_LMSdata.Length-2] == "***")
                        {
                            NUM = int.Parse(line_LMSdata[line_LMSdata.Length - 1]);
                            valid = true;
                        }
                    }
                    if (!valid) //数据无效
                    {
                        richTextBox_Show.SelectionColor = Color.Red;
                        richTextBox_Show.AppendText("“LMS111rawdata.txt”文件数据无效！\n");
                        richTextBox_Show.ScrollToCaret();
                    }
                    else //数据有效
                    {
                        string str;
                        string[] sub_str;
                        int myi;
                        int total = line_LMSdata.Length;

                        LMS_dataX_history = new double[NUM, LMS_NUM_cali];
                        LMS_dataY_history = new double[NUM, LMS_NUM_cali];
                        #region 读取激光数据
                        int point_index = 0;
                        int pos_index = -1;
                        for (int k = 0; k < total; k++)
                        {
                            str = line_LMSdata[k];
                            myi = str.Length;
                            if (myi > 0 && str[0] == '*')
                            {
                                pos_index++;
                                point_index = 0;
                            }
                            else if (myi > 4)
                            {
                                sub_str = str.Split(' ');
                                sub_str[0] = sub_str[0].Remove(0, 1);
                                sub_str[0] = sub_str[0].Remove(sub_str[0].Length - 1, 1);//去除sub[0]的首尾字符
                                if (sub_str.Length >= 2)
                                {
                                    int angle_index = Int32.Parse(sub_str[0]);
                                    double angle = (LMS_StartAngle + angle_index * LMS_Resolution) * Math.PI / 180;
                                    double dis = double.Parse(sub_str[1]);
                                    double coor_x = dis * Math.Cos(angle);
                                    double coor_y = dis * Math.Sin(angle);
                                    LMS_dataX_history[pos_index, point_index] = coor_x;
                                    LMS_dataY_history[pos_index, point_index] = coor_y;

                                    point_index++;
                                }
                            }
                        }

                        //读取每一段的界限
                        LMS_dataLimt_history = new int[NUM, 2];
                        int row_count = -1;
                        int limtMax = 0;
                        int limtMin = LMS_NUM_cali - 1;
                        total = line_LMSdata_valid.Length;
                        for (int i = 0; i < total; ++i)
                        {
                            str = line_LMSdata_valid[i];
                            myi = str.Length;
                            if (myi == 0)
                            {
                                LMS_dataLimt_history[row_count, 0] = limtMin;
                                LMS_dataLimt_history[row_count, 1] = limtMax;
                            }
                            else if (myi > 0 && str[0] == '*')
                            {
                                row_count++;
                                limtMax = 0;
                                limtMin = LMS_NUM_cali - 1;
                            }
                            else if (myi >= 3)
                            {
                                sub_str = str.Split(' ');
                                if (sub_str.Length >= 2)
                                {
                                    int angle_index = Int32.Parse(sub_str[0]);
                                    if (angle_index > limtMax)
                                        limtMax = angle_index;
                                    if (angle_index < limtMin)
                                        limtMin = angle_index;
                                }
                            }
                        }

                        #endregion
                        numericUpDown_LMSIndex.Maximum = NUM;
                        numericUpDown_LMSIndex.Enabled = true;

                        //显示一段数据
                        ChartClear();
                        int index = (int)numericUpDown_LMSIndex.Value;
                        double[] LMS_dataX_temp = new double[LMS_NUM_cali];
                        double[] LMS_dataY_temp = new double[LMS_NUM_cali];
                        for (int i = 0; i < LMS_NUM_cali; ++i)
                        {
                            LMS_dataX_temp[i] = LMS_dataX_history[index - 1, i];
                            LMS_dataY_temp[i] = LMS_dataY_history[index - 1, i];
                        }
                        double Ymax = LMS_dataY_temp.Max();
                        double Xwide = Math.Max(Math.Abs(LMS_dataX_temp.Min()), Math.Abs(LMS_dataX_temp.Max()));
                        chart_LMS111data.ChartAreas[0].AxisX.Maximum = Xwide * 1.1;
                        chart_LMS111data.ChartAreas[0].AxisX.Minimum = -Xwide * 1.1;
                        chart_LMS111data.ChartAreas[0].AxisY.Maximum = Ymax * 1.1;
                        chart_LMS111data.ChartAreas[0].AxisY.Minimum = -10;

                        for (int i = 0; i < LMS_NUM_cali; ++i)
                        {
                            if (i >= LMS_dataLimt_history[0, 0] && i <= LMS_dataLimt_history[0, 1])
                                chart_LMS111data.Series[1].Points.AddXY(LMS_dataX_temp[i], LMS_dataY_temp[i]);
                            else
                                chart_LMS111data.Series[0].Points.AddXY(LMS_dataX_temp[i], LMS_dataY_temp[i]);
                        }
                    }
                }
                catch (System.Exception ex)
                {
                    richTextBox_Show.SelectionColor = Color.Red;
                    richTextBox_Show.AppendText(ex.ToString() + '\n');
                    richTextBox_Show.AppendText("读取激光数据失败！");
                    richTextBox_Show.ScrollToCaret();
                }
            }
            else if (LMS111_dataReceived) //实时采集
            {
                int target_min = Int32.Parse(textBox_ScaleMin.Text);
                int target_max = Int32.Parse(textBox_ScaleMax.Text);
                double[] LMS_dataX = new double[LMS_NUM_cali];
                double[] LMS_dataY = new double[LMS_NUM_cali];
                for (int i = 0; i < LMS_NUM_cali; ++i)
                {
                    double angle = (LMS_StartAngle + i * LMS_Resolution) * Math.PI / 180;
                    LMS_dataX[i] = LMS_data_cali[i] * Math.Cos(angle);
                    LMS_dataY[i] = LMS_data_cali[i] * Math.Sin(angle);
                }

                double Ymax = LMS_dataY.Max();
                double Xwide = Math.Max(Math.Abs(LMS_dataX.Min()), Math.Abs(LMS_dataX.Max()));
                chart_LMS111data.ChartAreas[0].AxisX.Maximum = Xwide * 1.1;
                chart_LMS111data.ChartAreas[0].AxisX.Minimum = -Xwide * 1.1;
                chart_LMS111data.ChartAreas[0].AxisY.Maximum = Ymax * 1.1;
                chart_LMS111data.ChartAreas[0].AxisY.Minimum = -10;
                ChartClear();
                for (int i = 0; i < LMS_NUM_cali; ++i)
                {
                    if (i >= target_min && i <= target_max)
                    {
                        chart_LMS111data.Series[0].Points.AddXY(LMS_dataX[i], LMS_dataY[i]);
                    }
                    else
                    {
                        chart_LMS111data.Series[1].Points.AddXY(LMS_dataX[i], LMS_dataY[i]);
                    }
                }
            }
            else
            {
                richTextBox_Show.SelectionColor = Color.Red;
                richTextBox_Show.AppendText("LMS111未打开,没有有效的LMS111数据！\n");
                richTextBox_Show.ScrollToCaret();
            }
        }

        private void checkBox_HistoryData_CheckedChanged(object sender, EventArgs e)
        {
            if (checkBox_HistoryData.Checked)
            {
                button_OpenChart.Text = "更新文件";
            }
            else
            {
                numericUpDown_LMSIndex.Enabled = false;
                button_OpenChart.Text = "更新数据";
            }
        }

        private void numericUpDown_LMSIndex_ValueChanged(object sender, EventArgs e)
        {
            //读取一段数据
            try
            {
                ChartClear();
                int index = (int)numericUpDown_LMSIndex.Value;
                double[] LMS_dataX_temp = new double[LMS_NUM_cali];
                double[] LMS_dataY_temp = new double[LMS_NUM_cali];
                for(int i=0; i<LMS_NUM_cali; ++i)
                {
                    LMS_dataX_temp[i] = LMS_dataX_history[index - 1, i];
                    LMS_dataY_temp[i] = LMS_dataY_history[index - 1, i];
                }
                double Ymax = LMS_dataY_temp.Max();
                double Xwide = Math.Max(Math.Abs(LMS_dataX_temp.Min()), Math.Abs(LMS_dataX_temp.Max()));
                chart_LMS111data.ChartAreas[0].AxisX.Maximum = Xwide * 1.1;
                chart_LMS111data.ChartAreas[0].AxisX.Minimum = -Xwide * 1.1;
                chart_LMS111data.ChartAreas[0].AxisY.Maximum = Ymax * 1.1;
                chart_LMS111data.ChartAreas[0].AxisY.Minimum = -10;
                for (int i = 0; i < LMS_NUM_cali; ++i)
                {
                    if (i >= LMS_dataLimt_history[index-1, 0] && i <= LMS_dataLimt_history[index-1, 1])
                        chart_LMS111data.Series[1].Points.AddXY(LMS_dataX_temp[i], LMS_dataY_temp[i]);
                    else
                        chart_LMS111data.Series[0].Points.AddXY(LMS_dataX_temp[i], LMS_dataY_temp[i]);
                }
            }
            catch (FormatException)
            {
                richTextBox_Show.SelectionColor = Color.Red;
                richTextBox_Show.AppendText("索引格式错误\n");
                richTextBox_Show.ScrollToCaret();
            }
            
        }

        private void toolStripButton_CaliResult_Click(object sender, EventArgs e)
        {
            if (toolStripButton_CaliResult.Text == "打开预览")
            {
                //打开预览效果
                toolStripButton_CaliResult.Text = "关闭预览";
                flag_previewfirst = true;
 
                flag_preview = true;
                currentMode = Mode.Preview;
                lmsState = LMSdataState.Preview;
            }
            else
            {
                //关闭预览效果
                toolStripButton_CaliResult.Text = "打开预览";

                flag_preview = false;
                currentMode = Mode.SavingFrames;
                lmsState = LMSdataState.Idle;
            }
        }

        private void button_UndoMunal_Click(object sender, EventArgs e)
        {
            frame_buffer_savepoint = frame_buffer_savepoint_pre;
            button_munal.Text = "手动";
            button_munal.Enabled = true;
        }

        private void toolStripButton_OffLineCali_Click(object sender, EventArgs e)
        {
            if (toolStripButton_OffLineCali.Text == "离线标定")
            {
                toolStripButton_OffLineCali.Text = "退出标定";
                //禁用摄像头和激光测距仪
                toolStripButton_ReadCamera.Enabled = false;
                toolStripButton_ReadLMS111.Enabled = false;
                button_ParameterSureOffline.Enabled = true;

            }
            else
            {
                toolStripButton_OffLineCali.Text = "离线标定";
                //启用摄像头和激光测距仪
                toolStripButton_ReadCamera.Enabled = true;
                toolStripButton_ReadLMS111.Enabled = true;
                button_ParameterSureOffline.Enabled = false;
            }
        }

        private void button_OfflineCaliStart_Click(object sender, EventArgs e)
        {
            if (button_OfflineCaliStart.Text == "运行")
            {
                button_OfflineCaliStart.Text = "重来";

                //set up the arrays needed
                Frame_array_buffer_offline = new Image<Gray, byte>[(int)numericUpDown_ImageNumOffline.Value];
                corners_object_list_offline = new MCvPoint3D32f[Frame_array_buffer_offline.Length][];
                corners_points_list_offline = new PointF[Frame_array_buffer_offline.Length][];

                imageCount_offline = 0;

                button_OfflineNext.Enabled = true;
            }
            else
            {
                button_OfflineCaliStart.Text = "运行";
            }
        }

        private void button_OfflineNext_Click(object sender, EventArgs e)
        {
            imageCount_offline++;
            if (imageCount_offline <= imageNum_offline)
                textBox_OfflineState.Text = "准备添加图像：" + imageCount_offline.ToString() + ".bmp";
            else
                textBox_OfflineState.Text = "准备计算参数";
            button_OfflineNext.Enabled = false;
            button_OfflineSure.Enabled = true;
        }

        private void button_OfflineSure_Click(object sender, EventArgs e)
        {
            try
            {
                if (imageCount_offline > imageNum_offline)
                {
                    for (int k = 0; k < imageNum_offline; k++)
                    {
                        corners_points_list_offline[k] = CameraCalibration.FindChessboardCorners(Frame_array_buffer_offline[k], patternSize_offline, Emgu.CV.CvEnum.CALIB_CB_TYPE.ADAPTIVE_THRESH);
                        //for accuracy
                        Frame_array_buffer_offline[k].FindCornerSubPix(corners_points_list_offline, new Size(11, 11), new Size(-1, -1), new MCvTermCriteria(30, 0.1));

                        //Fill our objects list with the real world mesurments for the intrinsic calculations
                        List<MCvPoint3D32f> object_list = new List<MCvPoint3D32f>();
                        for (int i = 0; i < height_offline; i++)
                        {
                            for (int j = 0; j < width_offline; j++)
                            {
                                object_list.Add(new MCvPoint3D32f(j * Convert.ToSingle(textBox_SquareWidth.Text)/1000, i * Convert.ToSingle(textBox_SquareHeight.Text)/1000, 0.0F)); //真实棋盘方格大小
                            }
                        }
                        corners_object_list_offline[k] = object_list.ToArray();
                    }

                    textBox_OfflineState.Text = "正在计算参数...";
                    
                    double error = CameraCalibration.CalibrateCamera(corners_object_list_offline, corners_points_list_offline, imgGray_offline.Size, IC, Emgu.CV.CvEnum.CALIB_TYPE.CV_CALIB_FIX_K3, out EX_Param);
                    //If Emgu.CV.CvEnum.CALIB_TYPE == CV_CALIB_USE_INTRINSIC_GUESS and/or CV_CALIB_FIX_ASPECT_RATIO are specified, some or all of fx, fy, cx, cy must be initialized before calling the function
                    //if you use FIX_ASPECT_RATIO and FIX_FOCAL_LEGNTH options, these values needs to be set in the intrinsic parameters before the CalibrateCamera function is called. Otherwise 0 values are used as default.
                    MessageBox.Show("Intrinsci Calculation Error: " + error.ToString(), "Results", MessageBoxButtons.OK, MessageBoxIcon.Information); //display the results to the user

                    //显示计算出的参数
                    textBox_Intrinsic11.Invoke(new EventHandler(delegate { textBox_Intrinsic11.Text = IC.IntrinsicMatrix.Data[0, 0].ToString("0.0000"); }));
                    textBox_Intrinsic12.Invoke(new EventHandler(delegate { textBox_Intrinsic12.Text = IC.IntrinsicMatrix.Data[0, 1].ToString("0.0000"); }));
                    textBox_Intrinsic13.Invoke(new EventHandler(delegate { textBox_Intrinsic13.Text = IC.IntrinsicMatrix.Data[0, 2].ToString("0.0000"); }));
                    textBox_Intrinsic21.Invoke(new EventHandler(delegate { textBox_Intrinsic21.Text = IC.IntrinsicMatrix.Data[1, 0].ToString("0.0000"); }));
                    textBox_Intrinsic22.Invoke(new EventHandler(delegate { textBox_Intrinsic22.Text = IC.IntrinsicMatrix.Data[1, 1].ToString("0.0000"); }));
                    textBox_Intrinsic23.Invoke(new EventHandler(delegate { textBox_Intrinsic23.Text = IC.IntrinsicMatrix.Data[1, 2].ToString("0.0000"); }));
                    textBox_Intrinsic31.Invoke(new EventHandler(delegate { textBox_Intrinsic31.Text = IC.IntrinsicMatrix.Data[2, 0].ToString("0.0000"); }));
                    textBox_Intrinsic32.Invoke(new EventHandler(delegate { textBox_Intrinsic32.Text = IC.IntrinsicMatrix.Data[2, 1].ToString("0.0000"); }));
                    textBox_Intrinsic33.Invoke(new EventHandler(delegate { textBox_Intrinsic33.Text = IC.IntrinsicMatrix.Data[2, 2].ToString("0.0000"); }));
                    textBox_DistortionK1.Invoke(new EventHandler(delegate { textBox_DistortionK1.Text = IC.DistortionCoeffs.Data[0, 0].ToString("0.0000"); }));
                    textBox_DistortionK2.Invoke(new EventHandler(delegate { textBox_DistortionK2.Text = IC.DistortionCoeffs.Data[1, 0].ToString("0.0000"); }));
                    textBox_DistortionK3.Invoke(new EventHandler(delegate { textBox_DistortionK3.Text = IC.DistortionCoeffs.Data[4, 0].ToString("0.0000"); }));
                    textBox_DistortionP1.Invoke(new EventHandler(delegate { textBox_DistortionP1.Text = IC.DistortionCoeffs.Data[2, 0].ToString("0.0000"); }));
                    textBox_DistortionP2.Invoke(new EventHandler(delegate { textBox_DistortionP2.Text = IC.DistortionCoeffs.Data[3, 0].ToString("0.0000"); }));

                    //保存相关参数
                    //初始化
                    file_EX_Param = new FileStream(mainwindows_calibration.path + "/doc/LMS111_EX_Param.txt", FileMode.Create);
                    file_IC_Param = new FileStream(mainwindows_calibration.path + "/doc/LMS111_IC_Param.txt", FileMode.Create);
                    write_EX_Param = new StreamWriter(file_EX_Param);
                    write_IC_Param = new StreamWriter(file_IC_Param);

                    write_IC_Param.WriteLine("IntrinsicMatrix");//保存内部矩阵
                    for (int i = 0; i < 3; i++)
                    {
                        for (int j = 0; j < 3; j++)
                        {
                            write_IC_Param.Write(IC.IntrinsicMatrix.Data[i, j].ToString() + "\t");
                        }
                        write_IC_Param.WriteLine();
                    }
                    write_IC_Param.WriteLine("DistortionCoeffs(K1,K2,P1,P2,K3)");
                    for (int i = 0; i < 5; i++)
                    {
                        write_IC_Param.Write(IC.DistortionCoeffs.Data[i, 0].ToString() + "\t");
                    }

                    write_EX_Param.WriteLine("ExtrinsicParameters " + imageNum_offline.ToString());
                    for (int i = 0; i < EX_Param.Length; i++)
                    {
                        write_EX_Param.WriteLine();
                        write_EX_Param.WriteLine("[" + i.ToString() + "]");
                        write_EX_Param.WriteLine("Rotation:");
                        for (int m = 0; m < 3; m++)
                        {
                            for (int n = 0; n < 3; n++)
                            {
                                write_EX_Param.Write(EX_Param[i].RotationVector.RotationMatrix.Data[m, n].ToString() + "\t");
                            }
                            write_EX_Param.WriteLine();
                        }

                        write_EX_Param.WriteLine("Translation:");
                        for (int m = 0; m < 3; m++)
                        {
                            write_EX_Param.Write(EX_Param[i].TranslationVector.Data[m, 0].ToString() + "\t");
                        }
                        write_EX_Param.WriteLine();
                    }

                    write_EX_Param.Close();
                    write_IC_Param.Close();
                }
                else
                {
                    textBox_OfflineState.Text = "正在添加图像：" + imageCount_offline.ToString() + ".bmp";
                    img_offline = new Image<Bgr, Byte>(mainwindows_calibration.path + "/doc/ImageSaved/" + imageCount_offline.ToString() + ".bmp");
                    imgGray_offline = img_offline.Convert<Gray, Byte>();

                    corners_offline = CameraCalibration.FindChessboardCorners(imgGray_offline, patternSize_offline, Emgu.CV.CvEnum.CALIB_CB_TYPE.ADAPTIVE_THRESH);
                    if (corners_offline != null) //chess board found
                    {
                        //make mesurments more accurate by using FindCornerSubPixel
                        imgGray_offline.FindCornerSubPix(new PointF[1][] { corners_offline }, new Size(11, 11), new Size(-1, -1), new MCvTermCriteria(30, 0.1));
                        Frame_array_buffer_offline[imageCount_offline - 1] = imgGray_offline.Copy(); //store the image
                        //draw the results
                        img_offline.Draw(new CircleF(corners_offline[0], 4), new Bgr(Color.Blue), 2);
                        for (int i = 1; i < corners_offline.Length; i++)
                        {
                            img_offline.Draw(new LineSegment2DF(corners_offline[i - 1], corners_offline[i]), line_colour_array_offline[i], 2);
                            img_offline.Draw(new CircleF(corners_offline[i], 4), new Bgr(Color.Blue), 2);
                        }

                        textBox_OfflineState.Text = "图像" + imageCount_offline.ToString() + ".bmp添加完成！";
                    }
                    corners_offline = null;

                    Main_Picturebox.Image = img_offline.Convert<Bgr, Byte>(); //显示
                    button_OfflineNext.Enabled = true;
                    button_OfflineSure.Enabled = false;
                }

            }
            catch (System.IO.FileNotFoundException)
            {
                richTextBox_Show.SelectionColor = Color.Red;
                richTextBox_Show.AppendText("标定图片" + imageCount_offline.ToString() + ".bmp" + "不存在！\n");
                richTextBox_Show.ScrollToCaret();
            }
            
        }

        private void button_AlphaCaliStart_Click(object sender, EventArgs e)
        {
            button_Position1.Enabled = true;
            lmsState = LMSdataState.LMSCali;
            ChartClear();
        }

        private void button_Position1_Click(object sender, EventArgs e)
        {
            flag_alpha1 = true;
            SendtoLMS111_single(); //向LMS111发送指令,获得激光数据
            button_Position2.Enabled = true;
        }

        private void button_Position2_Click(object sender, EventArgs e)
        {
            flag_alpha2 = true;
            SendtoLMS111_single(); //向LMS111发送指令,获得激光数据
            button_AlphaParaSure.Enabled = true;

            textBox_MoveDistance.Enabled = true;
            textBox_AlphaScaleMin.Enabled = true;
            textBox_AlphaScaleMax.Enabled = true;
        }

        private void button_AlphaParaSure_Click(object sender, EventArgs e)
        {
            button_AlphaCalculate.Enabled = true;
            //不能修改
            textBox_MoveDistance.Enabled = false;
            textBox_AlphaScaleMin.Enabled = false;
            textBox_AlphaScaleMax.Enabled = false;
        }

        private void button_AlphaCalculate_Click(object sender, EventArgs e)
        {
            int min = Convert.ToInt32(textBox_AlphaScaleMin.Text);
            int max = Convert.ToInt32(textBox_AlphaScaleMax.Text);
            double distance = Convert.ToDouble(textBox_MoveDistance.Text);
            int del = Convert.ToInt32(textBox_LMSCalInternal.Text);

            int num = (max - min) / del;
            double[] result = new double[num];
            int index = min;
            for (int i = 0; i < num; i++)
            {
                result[i] = CalculateAlpha(distance, LMS_data_alpha1[i + del], LMS_data_alpha1[i], LMS_data_alpha2[i + del], i + del, i);
            }

            alpha = result.Average();

            richTextBox_Show.SelectionColor = Color.Black;
            richTextBox_Show.AppendText("Alpha的标定结果为：" + alpha.ToString() + "\n");
            richTextBox_Show.ScrollToCaret();
        }

        private void button_Update_Click(object sender, EventArgs e)
        {
            int index_min = Convert.ToInt32(textBox_AlphaScaleMin.Text);
            int index_max = Convert.ToInt32(textBox_AlphaScaleMax.Text);

            chart_LMS111data.Series[1].Points.Clear();
            chart_LMS111data.Series[3].Points.Clear();
            for (int i = index_min; i <= index_max; ++i)
            {
                chart_LMS111data.Series[1].Points.AddXY(LMS_data_alpha1X[i], LMS_data_alpha1Y[i]);
                chart_LMS111data.Series[3].Points.AddXY(LMS_data_alpha2X[i], LMS_data_alpha2Y[i]);
            }
        }

        private void textBox_AlphaScaleMin_TextChanged(object sender, EventArgs e)
        {
            try
            {
                int min = Convert.ToInt32(textBox_AlphaScaleMin.Text);
                int max = Convert.ToInt32(textBox_AlphaScaleMax.Text);
                double distance = Convert.ToDouble(textBox_MoveDistance.Text);
                int del = Convert.ToInt32(textBox_LMSCalInternal.Text);

                if ((min <= max) && (min >= 0) && (min >= 0) && (max < LMS_NUM_cali) && (del <= (max-min)))
                {
                    button_Update.Enabled = true;
                    button_AlphaParaSure.Enabled = true;
                }
                else
                {
                    button_Update.Enabled = false;
                    button_AlphaParaSure.Enabled = false;
                }
            }
            catch (FormatException)
            {
                button_Update.Enabled = false;
                button_AlphaParaSure.Enabled = false;
            }
        }

        private void textBox_AlphaScaleMax_TextChanged(object sender, EventArgs e)
        {
            try
            {
                int min = Convert.ToInt32(textBox_AlphaScaleMin.Text);
                int max = Convert.ToInt32(textBox_AlphaScaleMax.Text);
                double distance = Convert.ToDouble(textBox_MoveDistance.Text);
                int del = Convert.ToInt32(textBox_LMSCalInternal.Text);

                if ((min <= max) && (min >= 0) && (min >= 0) && (max < LMS_NUM_cali) && (del <= (max - min)))
                {
                    button_Update.Enabled = true;
                    button_AlphaParaSure.Enabled = true;
                }
                else
                {
                    button_Update.Enabled = false;
                    button_AlphaParaSure.Enabled = false;
                }
            }
            catch (FormatException)
            {
                button_Update.Enabled = false;
                button_AlphaParaSure.Enabled = false;
            }
        }

        private void textBox_MoveDistance_TextChanged(object sender, EventArgs e)
        {
            try
            {
                int min = Convert.ToInt32(textBox_AlphaScaleMin.Text);
                int max = Convert.ToInt32(textBox_AlphaScaleMax.Text);
                double distance = Convert.ToDouble(textBox_MoveDistance.Text);
                int del = Convert.ToInt32(textBox_LMSCalInternal.Text);

                if ((min <= max) && (min >= 0) && (min >= 0) && (max < LMS_NUM_cali) && (del <= (max - min)))
                {
                    button_Update.Enabled = true;
                    button_AlphaParaSure.Enabled = true;
                }
                else
                {
                    button_Update.Enabled = false;
                    button_AlphaParaSure.Enabled = false;
                }
            }
            catch (FormatException)
            {
                button_Update.Enabled = false;
                button_AlphaParaSure.Enabled = false;
            }
        }

        private void textBox_LMSCalInternal_TextChanged(object sender, EventArgs e)
        {
            try
            {
                int min = Convert.ToInt32(textBox_AlphaScaleMin.Text);
                int max = Convert.ToInt32(textBox_AlphaScaleMax.Text);
                double distance = Convert.ToDouble(textBox_MoveDistance.Text);
                int del = Convert.ToInt32(textBox_LMSCalInternal.Text);

                if ((min <= max) && (min >= 0) && (min >= 0) && (max < LMS_NUM_cali) && (del <= (max - min)))
                {
                    button_Update.Enabled = true;
                    button_AlphaParaSure.Enabled = true;
                }
                else
                {
                    button_Update.Enabled = false;
                    button_AlphaParaSure.Enabled = false;
                }
            }
            catch (FormatException)
            {
                button_Update.Enabled = false;
                button_AlphaParaSure.Enabled = false;
            }
        }

        private void toolStripButton_SaveParameters_Click(object sender, EventArgs e)
        {
            xml_calibration.Load(mainwindows_calibration.path + "/doc/ParametersModel.xml");
            XmlElement xml_root = xml_calibration.DocumentElement;
            //增加节点
            XmlElement newIntrinsics = xml_calibration.CreateElement("intrinsics");
            XmlElement newDistortion = xml_calibration.CreateElement("distortion");
            XmlElement newAlpha = xml_calibration.CreateElement("alpha");
            XmlElement newRotation = xml_calibration.CreateElement("rotation");
            XmlElement newDeviation = xml_calibration.CreateElement("deviation");
            //元素值
            string intrinsics_data = textBox_Intrinsic11.Text + " " + textBox_Intrinsic12.Text + " " + textBox_Intrinsic13.Text + " " +
                                     textBox_Intrinsic21.Text + " " + textBox_Intrinsic22.Text + " " + textBox_Intrinsic23.Text + " " +
                                     textBox_Intrinsic31.Text + " " + textBox_Intrinsic32.Text + " " + textBox_Intrinsic33.Text;
            string distortion_data = textBox_DistortionK1.Text + " " + textBox_DistortionK2.Text + " " + textBox_DistortionP1.Text + " " +
                                     textBox_DistortionP2.Text + " " + textBox_DistortionK3.Text;
            string alpha_data = textBox_alpha.Text;
            string rotation_data = textBox_Rotation11.Text + " " + textBox_Rotation12.Text + " " + textBox_Rotation13.Text + " " +
                                   textBox_Rotation21.Text + " " + textBox_Rotation22.Text + " " + textBox_Rotation23.Text + " " +
                                   textBox_Rotation31.Text + " " + textBox_Rotation32.Text + " " + textBox_Rotation33.Text;
            string deviation_data = textBox_DeviationX.Text + " " + textBox_DeviationY.Text + " " + textBox_DeviationZ.Text;
            XmlText intrinsicsText = xml_calibration.CreateTextNode(intrinsics_data);
            XmlText distortionText = xml_calibration.CreateTextNode(distortion_data);
            XmlText alphaText = xml_calibration.CreateTextNode(alpha_data);
            XmlText rotationText = xml_calibration.CreateTextNode(rotation_data);
            XmlText deviationText = xml_calibration.CreateTextNode(deviation_data);

            //添加元素
            xml_root.AppendChild(newIntrinsics);
            xml_root.AppendChild(newDistortion);
            xml_root.AppendChild(newAlpha);
            xml_root.AppendChild(newRotation);
            xml_root.AppendChild(newDeviation);
            newIntrinsics.AppendChild(intrinsicsText);
            newDistortion.AppendChild(distortionText);
            newAlpha.AppendChild(alphaText);
            newRotation.AppendChild(rotationText);
            newDeviation.AppendChild(deviationText);

            xml_calibration.Save(mainwindows_calibration.path + "/doc/Calibration.xml");
            MessageBox.Show("成功存储参数!");
        }

        private void button_AdjustPrepare_Click(object sender, EventArgs e)
        {
            rotation_AdjustOri[0, 0] = Convert.ToDouble(textBox_Rotation11.Text);
            rotation_AdjustOri[0, 1] = Convert.ToDouble(textBox_Rotation12.Text);
            rotation_AdjustOri[0, 2] = Convert.ToDouble(textBox_Rotation13.Text);
            rotation_AdjustOri[1, 0] = Convert.ToDouble(textBox_Rotation21.Text);
            rotation_AdjustOri[1, 1] = Convert.ToDouble(textBox_Rotation22.Text);
            rotation_AdjustOri[1, 2] = Convert.ToDouble(textBox_Rotation23.Text);
            rotation_AdjustOri[2, 0] = Convert.ToDouble(textBox_Rotation31.Text);
            rotation_AdjustOri[2, 1] = Convert.ToDouble(textBox_Rotation32.Text);
            rotation_AdjustOri[2, 2] = Convert.ToDouble(textBox_Rotation33.Text);

            AdjustRotationLoad();
        }

        private void button_Transform_Click(object sender, EventArgs e)
        {
            Matrix<double> XMatrix = new Matrix<double>(3, 3);
            Matrix<double> YMatrix = new Matrix<double>(3, 3);
            Matrix<double> ZMatrix = new Matrix<double>(3, 3);
            double angle_x = Convert.ToDouble(textBox_XAdjust.Text) * Math.PI / 180;
            double angle_y = Convert.ToDouble(textBox_YAdjust.Text) * Math.PI / 180;
            double angle_z = Convert.ToDouble(textBox_ZAdjust.Text) * Math.PI / 180;

            //顺时针偏转
            XMatrix[0, 0] = 1;
            XMatrix[0, 1] = 0;
            XMatrix[0, 2] = 0;
            XMatrix[1, 0] = 0;
            XMatrix[1, 1] = Math.Cos(angle_x);
            XMatrix[1, 2] = -Math.Sin(angle_x);
            XMatrix[2, 0] = 0;
            XMatrix[2, 1] = Math.Sin(angle_x);
            XMatrix[2, 2] = Math.Cos(angle_x);

            YMatrix[0, 0] = Math.Cos(angle_y);
            YMatrix[0, 1] = 0;
            YMatrix[0, 2] = Math.Sin(angle_y);
            YMatrix[1, 0] = 0;
            YMatrix[1, 1] = 1;
            YMatrix[1, 2] = 0;
            YMatrix[2, 0] = -Math.Sin(angle_y);
            YMatrix[2, 1] = 0;
            YMatrix[2, 2] = Math.Cos(angle_y);

            ZMatrix[0, 0] = Math.Cos(angle_z);
            ZMatrix[0, 1] = -Math.Sin(angle_z);
            ZMatrix[0, 2] = 0;
            ZMatrix[1, 0] = Math.Sin(angle_z);
            ZMatrix[1, 1] = Math.Cos(angle_z);
            ZMatrix[1, 2] = 0;
            ZMatrix[2, 0] = 0;
            ZMatrix[2, 1] = 0;
            ZMatrix[2, 2] = 1;

            rotation_Adjust = rotation_AdjustOri;
            rotation_Adjust = rotation_Adjust.Mul(XMatrix);
            rotation_Adjust = rotation_Adjust.Mul(YMatrix);
            rotation_Adjust = rotation_Adjust.Mul(ZMatrix);

            textBox_AdjustRotation11.Text = rotation_Adjust[0, 0].ToString("0.0000");
            textBox_AdjustRotation12.Text = rotation_Adjust[0, 1].ToString("0.0000");
            textBox_AdjustRotation13.Text = rotation_Adjust[0, 2].ToString("0.0000");
            textBox_AdjustRotation21.Text = rotation_Adjust[1, 0].ToString("0.0000");
            textBox_AdjustRotation22.Text = rotation_Adjust[1, 1].ToString("0.0000");
            textBox_AdjustRotation23.Text = rotation_Adjust[1, 2].ToString("0.0000");
            textBox_AdjustRotation31.Text = rotation_Adjust[2, 0].ToString("0.0000");
            textBox_AdjustRotation32.Text = rotation_Adjust[2, 1].ToString("0.0000");
            textBox_AdjustRotation33.Text = rotation_Adjust[2, 2].ToString("0.0000");
        }

        private void button_AdjustApply_Click(object sender, EventArgs e)
        {
            textBox_Rotation11.Text = textBox_AdjustRotation11.Text;
            textBox_Rotation12.Text = textBox_AdjustRotation12.Text;
            textBox_Rotation13.Text = textBox_AdjustRotation13.Text;
            textBox_Rotation21.Text = textBox_AdjustRotation21.Text;
            textBox_Rotation22.Text = textBox_AdjustRotation22.Text;
            textBox_Rotation23.Text = textBox_AdjustRotation23.Text;
            textBox_Rotation31.Text = textBox_AdjustRotation31.Text;
            textBox_Rotation32.Text = textBox_AdjustRotation32.Text;
            textBox_Rotation33.Text = textBox_AdjustRotation33.Text;
        }

        private void button_AdjustUndo_Click(object sender, EventArgs e)
        {
            textBox_Rotation11.Text = rotation_AdjustOri[0, 0].ToString();
            textBox_Rotation12.Text = rotation_AdjustOri[0, 1].ToString();
            textBox_Rotation13.Text = rotation_AdjustOri[0, 2].ToString();
            textBox_Rotation21.Text = rotation_AdjustOri[1, 0].ToString();
            textBox_Rotation22.Text = rotation_AdjustOri[1, 1].ToString();
            textBox_Rotation23.Text = rotation_AdjustOri[1, 2].ToString();
            textBox_Rotation31.Text = rotation_AdjustOri[2, 0].ToString();
            textBox_Rotation32.Text = rotation_AdjustOri[2, 1].ToString();
            textBox_Rotation33.Text = rotation_AdjustOri[2, 2].ToString();
        }
        #endregion

        #region 自定义函数
        //初始化标定参数
        private void CalibrationParametersLoad(string doc_name)
        {
            XmlDocument xml_ReadCalibration = new XmlDocument();
            xml_ReadCalibration.Load(mainwindows_calibration.path + "/doc/" + doc_name);
            XmlNode currentNode;
            string[] temp;

            //标定参数加载
            currentNode = xml_ReadCalibration.SelectSingleNode("//intrinsics");
            string intrinsicText = currentNode.InnerText;
            temp = intrinsicText.Split(' ');
            textBox_Intrinsic11.Text = temp[0];
            textBox_Intrinsic12.Text = temp[1];
            textBox_Intrinsic13.Text = temp[2];
            textBox_Intrinsic21.Text = temp[3];
            textBox_Intrinsic22.Text = temp[4];
            textBox_Intrinsic23.Text = temp[5];
            textBox_Intrinsic31.Text = temp[6];
            textBox_Intrinsic32.Text = temp[7];
            textBox_Intrinsic33.Text = temp[8];

            currentNode = xml_ReadCalibration.SelectSingleNode("//distortion");
            string distortionText = currentNode.InnerText;
            temp = distortionText.Split(' ');
            textBox_DistortionK1.Text = temp[0];
            textBox_DistortionK2.Text = temp[1];
            textBox_DistortionK3.Text = temp[4];
            textBox_DistortionP1.Text = temp[2];
            textBox_DistortionP2.Text = temp[3];

            currentNode = xml_ReadCalibration.SelectSingleNode("//alpha");
            string alphaText = currentNode.InnerText;
            textBox_alpha.Text = alphaText;

            currentNode = xml_ReadCalibration.SelectSingleNode("//rotation");
            string rotationText = currentNode.InnerText;
            temp = rotationText.Split(' ');
            textBox_Rotation11.Text = temp[0];
            textBox_Rotation12.Text = temp[1];
            textBox_Rotation13.Text = temp[2];
            textBox_Rotation21.Text = temp[3];
            textBox_Rotation22.Text = temp[4];
            textBox_Rotation23.Text = temp[5];
            textBox_Rotation31.Text = temp[6];
            textBox_Rotation32.Text = temp[7];
            textBox_Rotation33.Text = temp[8];

            currentNode = xml_ReadCalibration.SelectSingleNode("//deviation");
            string deviationText = currentNode.InnerText;
            temp = deviationText.Split(' ');
            textBox_DeviationX.Text = temp[0];
            textBox_DeviationY.Text = temp[1];
            textBox_DeviationZ.Text = temp[2];
        }

        private void AdjustRotationLoad()
        {
            textBox_AdjustRotation11.Text = textBox_Rotation11.Text;
            textBox_AdjustRotation12.Text = textBox_Rotation12.Text;
            textBox_AdjustRotation13.Text = textBox_Rotation13.Text;
            textBox_AdjustRotation21.Text = textBox_Rotation21.Text;
            textBox_AdjustRotation22.Text = textBox_Rotation22.Text;
            textBox_AdjustRotation23.Text = textBox_Rotation23.Text;
            textBox_AdjustRotation31.Text = textBox_Rotation31.Text;
            textBox_AdjustRotation32.Text = textBox_Rotation32.Text;
            textBox_AdjustRotation33.Text = textBox_Rotation33.Text;
        }

        //测试文本框
        private void TestText()
        {
            double temp1 = Convert.ToDouble(textBox_Intrinsic11.Text);
            double temp2 = Convert.ToDouble(textBox_Intrinsic12.Text);
            double temp3 = Convert.ToDouble(textBox_Intrinsic13.Text);
            double temp4 = Convert.ToDouble(textBox_Intrinsic21.Text);
            double temp5 = Convert.ToDouble(textBox_Intrinsic22.Text);
            double temp6 = Convert.ToDouble(textBox_Intrinsic23.Text);
            double temp7 = Convert.ToDouble(textBox_Intrinsic31.Text);
            double temp8 = Convert.ToDouble(textBox_Intrinsic32.Text);
            double temp9 = Convert.ToDouble(textBox_Intrinsic33.Text);
            double temp10 = Convert.ToDouble(textBox_DistortionK1.Text);
            double temp11 = Convert.ToDouble(textBox_DistortionK2.Text);
            double temp12 = Convert.ToDouble(textBox_DistortionK3.Text);
            double temp13 = Convert.ToDouble(textBox_DistortionP1.Text);
            double temp14 = Convert.ToDouble(textBox_DistortionP2.Text);
            double temp15 = Convert.ToDouble(textBox_alpha.Text);
            double temp16 = Convert.ToDouble(textBox_Rotation11.Text);
            double temp17 = Convert.ToDouble(textBox_Rotation12.Text);
            double temp18 = Convert.ToDouble(textBox_Rotation13.Text);
            double temp19 = Convert.ToDouble(textBox_Rotation21.Text);
            double temp20 = Convert.ToDouble(textBox_Rotation22.Text);
            double temp21 = Convert.ToDouble(textBox_Rotation23.Text);
            double temp22 = Convert.ToDouble(textBox_Rotation31.Text);
            double temp23 = Convert.ToDouble(textBox_Rotation32.Text);
            double temp24 = Convert.ToDouble(textBox_Rotation33.Text);
            double temp25 = Convert.ToDouble(textBox_DeviationX.Text);
            double temp26 = Convert.ToDouble(textBox_DeviationY.Text);
            double temp27 = Convert.ToDouble(textBox_DeviationZ.Text);
        }

        //清空chart控件数据
        private void ChartClear()
        {
            chart_LMS111data.Series[0].Points.Clear();
            chart_LMS111data.Series[1].Points.Clear();
            chart_LMS111data.Series[2].Points.Clear();
            chart_LMS111data.Series[3].Points.Clear();
        }

        //计算倾角
        private double CalculateAlpha(double x, double d1, double d2, double d3, double index1, double index2)//x为平移距离,d1为左边点近距离值,d2为右边点的近距离值,d3为左边点的远距离值,index1为左边点序号,index2为右边点序号
        {
            double alpha = 0;
            double theta1, theta2;

            theta1 = (LMS_StartAngle + index1 * LMS_Resolution) * Math.PI / 180;
            theta2 = (LMS_StartAngle + index2 * LMS_Resolution) * Math.PI / 180;

            double a = d1 * d1 + d2 * d2 - 2 * d1 * d2 * Math.Cos(theta1 - theta2);
            double b = d2 * (d3 - d1) * Math.Sin(theta1 - theta2);
            double c = x * Math.Sqrt(a) / b;
            alpha = Math.Acos(c);

            return alpha;
        }

        //向LMS111发送指令
        private void SendtoLMS111_single()
        {
            LMS111_serial.DiscardInBuffer();
            LMS111_serial.Write(cmd_single, 0, cmd_single.Length);
        }

        //读取标定数据
        private void ReadCaliParameters()
        {
            intrinsic.Data[0, 0] = Convert.ToDouble(textBox_Intrinsic11.Text);
            intrinsic.Data[0, 1] = Convert.ToDouble(textBox_Intrinsic12.Text);
            intrinsic.Data[0, 2] = Convert.ToDouble(textBox_Intrinsic13.Text);
            intrinsic.Data[1, 0] = Convert.ToDouble(textBox_Intrinsic21.Text);
            intrinsic.Data[1, 1] = Convert.ToDouble(textBox_Intrinsic22.Text);
            intrinsic.Data[1, 2] = Convert.ToDouble(textBox_Intrinsic23.Text);
            intrinsic.Data[2, 0] = Convert.ToDouble(textBox_Intrinsic31.Text);
            intrinsic.Data[2, 1] = Convert.ToDouble(textBox_Intrinsic32.Text);
            intrinsic.Data[2, 2] = Convert.ToDouble(textBox_Intrinsic33.Text);

            distortion.Data[0, 0] = Convert.ToDouble(textBox_DistortionK1.Text);
            distortion.Data[1, 0] = Convert.ToDouble(textBox_DistortionK2.Text);
            distortion.Data[2, 0] = Convert.ToDouble(textBox_DistortionP1.Text);
            distortion.Data[3, 0] = Convert.ToDouble(textBox_DistortionP2.Text);
            distortion.Data[4, 0] = Convert.ToDouble(textBox_DistortionK3.Text);

            alpha = Convert.ToDouble(textBox_alpha.Text);

            rotation.Data[0, 0] = Convert.ToDouble(textBox_Rotation11.Text);
            rotation.Data[0, 1] = Convert.ToDouble(textBox_Rotation12.Text);
            rotation.Data[0, 2] = Convert.ToDouble(textBox_Rotation13.Text);
            rotation.Data[1, 0] = Convert.ToDouble(textBox_Rotation21.Text);
            rotation.Data[1, 1] = Convert.ToDouble(textBox_Rotation22.Text);
            rotation.Data[1, 2] = Convert.ToDouble(textBox_Rotation23.Text);
            rotation.Data[2, 0] = Convert.ToDouble(textBox_Rotation31.Text);
            rotation.Data[2, 1] = Convert.ToDouble(textBox_Rotation32.Text);
            rotation.Data[2, 2] = Convert.ToDouble(textBox_Rotation33.Text);

            deviation.Data[0, 0] = Convert.ToDouble(textBox_DeviationX.Text);
            deviation.Data[0, 1] = Convert.ToDouble(textBox_DeviationY.Text);
            deviation.Data[0, 2] = Convert.ToDouble(textBox_DeviationZ.Text);

            IC.IntrinsicMatrix = intrinsic;
            IC.DistortionCoeffs = distortion;
        }

        //转换激光数据点
        private void LMSdateTransform()
        {
            double LMS_angle;
            double r_angle;
            double theta1;
            double theta2;

            double[] LMS_X_LaserPlan = new double[LMS_NUM_cali];//激光点在激光平面坐标系上的X坐标
            double[] LMS_Y_LaserPlan = new double[LMS_NUM_cali];//激光点在激光平面坐标系上的Y坐标
            double[] LMS_Z_LaserPlan = new double[LMS_NUM_cali];//激光点在激光平面坐标系上的Z坐标

            for (int n = 0; n < LMS_NUM_cali; n++)
            {
                LMS_angle = (n * LMS_Resolution + LMS_StartAngle) / 180.0 * Math.PI; //这是激光测距仪特定设置下的计算,要实现通用化,只要改变这个公式
                theta1 = Math.PI / 2 - LMS_angle;
                r_angle = Math.Asin(Math.Sin(alpha) * Math.Cos(theta1));
                theta2 = Math.Acos(Math.Cos(theta1) * Math.Cos(alpha) / Math.Cos(r_angle));//此时theta2一定为正,但是需要判断正负,为了求正确坐标
                if (n > (LMS_NUM_cali / 2))
                {
                    theta2 = -theta2;
                }

                LMS_X[n] = LMS_data_cali[n] / 1000.0 * Math.Sin(theta2) * Math.Cos(r_angle);//用于定位的数据
                LMS_Z[n] = LMS_data_cali[n] / 1000.0 * Math.Cos(theta2) * Math.Cos(r_angle);//用于定位的数据

                LMS_X_LaserPlan[n] = LMS_data_cali[n] / 1000.0 * Math.Sin(theta1);
                LMS_Y_LaserPlan[n] = -LMS_data_cali[n] / 1000.0 * Math.Cos(theta1) * Math.Cos(alpha) * Math.Sin(alpha);
                LMS_Z_LaserPlan[n] = LMS_data_cali[n] / 1000.0 * Math.Cos(theta1) * Math.Cos(alpha) * Math.Cos(alpha);

                Pf[0] = LMS_X_LaserPlan[n];
                Pf[1] = LMS_Y_LaserPlan[n];
                Pf[2] = LMS_Z_LaserPlan[n];

                double[] temp_LMS = new double[3];
                double[] temp_qs = new double[3];

                /***************************************************************
			    Pl = R * Pc + T  
			    其中 Pl为激光测距仪坐标系下点的坐标，在程序中为Pf[3]；
				 Pc为摄像头坐标系下点的坐标，在程序中为temp_qs[3]；
				 T 为激光测距仪坐标系下摄像头坐标原点的坐标，在程序中为V[3]；
				 R 为旋转矩阵，R_INV为逆矩阵
			    ****************************************************************/
                for (int i = 0; i < 3; i++)
                {
                    temp_LMS[i] = Pf[i] - deviation.Data[0, i];
                }

                for (int i = 0; i < 3; i++)
                {
                    temp_qs[i] = 0;
                    for (int j = 0; j < 3; j++)
                    {
                        temp_qs[i] += rotation.Data[i, j] * temp_LMS[j];
                    }
                }

                /*****************************************
			    p = K*Pc  摄像头坐标系中的点转换到图像平面
			    程序中K为Ins[3][3]矩阵, p为P_transfer[3]
			    *****************************************/
                for (int i = 0; i < 3; i++)
                {
                    P_transfer[i] = 0;
                    for (int j = 0; j < 3; j++)
                    {
                        P_transfer[i] += intrinsic.Data[i, j] * temp_qs[j];
                    }
                }
                for (int i = 0; i < 3; i++)
                {
                    P_transfer[i] = P_transfer[i] / P_transfer[2];
                }

                LMS_u[n] = (int)P_transfer[0];
                LMS_v[n] = (int)P_transfer[1];
            }
        }

        private unsafe void Calculate_Ex_Para() 
        {
            GC.Collect();

            try
            {
                string[] line_EX = File.ReadAllLines(mainwindows_calibration.path + "/doc/LMS111_EX_Param.txt");
                string[] line_LMSdata = File.ReadAllLines(mainwindows_calibration.path + "/doc/LMS111data.txt");
                bool valid = false;
                int NUM = 0; //图像个数
                if (line_EX.Length >= 1)
                {
                    string[] temp = line_EX[0].Split(' ');
                    if (temp.Length == 2)
                        if (temp[0] == "ExtrinsicParameters")
                        {
                            NUM = int.Parse(temp[1]);
                            valid = true;
                        }
                }

                if (!valid) //数据无效
                {
                    richTextBox_Show.SelectionColor = Color.Red;
                    richTextBox_Show.AppendText("“LMS111_EX_Param.txt”等文件数据无效！\n");
                    richTextBox_Show.ScrollToCaret();
                }
                else //数据有效
                {
                    Matrix<double> RR = new Matrix<double>(NUM, 3);//存外矩阵第三列
                    Matrix<double> TT = new Matrix<double>(NUM, 3);//存相对位置
                    Matrix<double> NN = new Matrix<double>(NUM, 3);//存法向量
                    double[,] para1 = new double[NUM, 9];    //存外矩阵R
                    double[,] para2 = new double[NUM, 3];  //存相对位置
                    double[,] para3 = new double[NUM, 3];  //存外矩阵第三列

                    int tem_count = 0;
                    string str;
                    string[] sub_str;
                    int myi;
                    //START to READ
                    richTextBox_Show.SelectionColor = Color.Black;
                    richTextBox_Show.AppendText("读取相关参数...\n");
                    richTextBox_Show.ScrollToCaret();

                    #region 读取旋转矩阵和平移向量
                    for (int k = 0; k < NUM; k++)
                    {
                        str = line_EX[8 * k + 4];
                        sub_str = str.Split('	');
                        myi = sub_str.Length;
                        if (myi >= 3)
                        {
                            for (int j = 0; j < 3; j++)
                                para1[k, j + 3 * tem_count] = double.Parse(sub_str[j] + 3 * tem_count);
                        }
                        tem_count = tem_count + 1;

                        str = line_EX[8 * k + 5];
                        sub_str = str.Split('	');
                        myi = sub_str.Length;
                        if (myi >= 3)
                        {
                            for (int j = 0; j < 3; j++)
                                para1[k, j + 3 * tem_count] = double.Parse(sub_str[j]);
                        }
                        tem_count = tem_count + 1;

                        str = line_EX[8 * k + 6];
                        sub_str = str.Split('	');
                        myi = sub_str.Length;
                        if (myi >= 3)
                        {
                            for (int j = 0; j < 3; j++)
                                para1[k, j + 3 * tem_count] = double.Parse(sub_str[j]);
                        }
                        tem_count = 0;
                        para3[k, 0] = para1[k, 2];
                        para3[k, 1] = para1[k, 5];
                        para3[k, 2] = para1[k, 8];

                        str = line_EX[8 * k + 8];
                        sub_str = str.Split('	');
                        myi = sub_str.Length;
                        if (myi >= 3)
                        {
                            for (int j = 0; j < 3; j++)
                                para2[k, j] = double.Parse(sub_str[j]);
                        }
                    }
                    for (int k = 0; k < NUM; k++)
                    {
                        for (int p = 0; p < 3; p++)
                        {
                            RR[k, p] = para3[k, p];
                        }
                    }
                    for (int k = 0; k < NUM; k++)
                    {
                        for (int p = 0; p < 3; p++)
                        {
                            TT[k, p] = para2[k, p];
                        }
                    }
                    #endregion
                    for (int k = 0; k < NUM; k++)//计算法向量
                    {
                        Matrix<double> tem = TT.GetRow(k).Transpose();
                        Matrix<double> para = RR.GetRow(k) * tem;
                        double c = (double)para[0, 0];
                        Matrix<double> temx = RR.GetRow(k) * c;
                        NN[k, 0] = -temx[0, 0];
                        NN[k, 1] = -temx[0, 1];
                        NN[k, 2] = -temx[0, 2];
                    }
                    //到此处可以计算出的是法向量

                    richTextBox_Show.SelectionColor = Color.Black;
                    richTextBox_Show.AppendText("读取激光数据...\n");
                    richTextBox_Show.ScrollToCaret();

                    int total = line_LMSdata.Length;
                    int points_num = total - NUM * 2;
                    Matrix<double> LD = new Matrix<double>(points_num, 9);
                    Matrix<double> D = new Matrix<double>(points_num, 1);
                    #region 读取激光数据
                    int point_num = 0;
                    int pos_num = -1;
                    for (int k = 0; k < total; k++)
                    {
                        str = line_LMSdata[k];
                        myi = str.Length;
                        if (myi > 0 && str[0] == '*')
                        {
                            pos_num = pos_num + 1;
                        }
                        else if (myi > 4)
                        {
                            sub_str = str.Split(' ');
                            if (sub_str.Length >= 2)
                            {
                                double angle_index = double.Parse(sub_str[0]);
                                double angle = (LMS_StartAngle + angle_index * LMS_Resolution) * Math.PI / 180;
                                double dis = double.Parse(sub_str[1]);
                                double coor_x = dis * Math.Cos(angle) / 1000;
                                double coor_y = dis * Math.Sin(angle) / 1000;
                                LD[point_num, 0] = NN[pos_num, 0] * coor_x;
                                LD[point_num, 1] = NN[pos_num, 0] * coor_y;
                                LD[point_num, 2] = NN[pos_num, 0];
                                LD[point_num, 3] = NN[pos_num, 1] * coor_x;
                                LD[point_num, 4] = NN[pos_num, 1] * coor_y;
                                LD[point_num, 5] = NN[pos_num, 1];
                                LD[point_num, 6] = NN[pos_num, 2] * coor_x;
                                LD[point_num, 7] = NN[pos_num, 2] * coor_y;
                                LD[point_num, 8] = NN[pos_num, 2];
                                Matrix<double> tem1 = NN.GetRow(pos_num);
                                Matrix<double> tem2 = tem1.Transpose();
                                Matrix<double> chengji = tem1 * tem2;
                                D[point_num, 0] = (double)chengji[0, 0];
                                point_num = point_num + 1;
                            }
                        }
                    }
                    #endregion

                    int interval = point_num / 100;
                    richTextBox_Show.SelectionColor = Color.Black;
                    richTextBox_Show.AppendText("方程个数：" + point_num.ToString() + "\n");
                    richTextBox_Show.AppendText("间隔：" + interval.ToString() + "\n");
                    richTextBox_Show.AppendText("计算中... \n");
                    richTextBox_Show.ScrollToCaret();

                    //抽取其中的100个方程求解最小二乘解
                    double[,] PINV = new double[100, 9];
                    double[] PD = new double[100];
                    for (int i = 0; i < 100; i++)
                    {
                        for (int j = 0; j < 9; j++)
                        {
                            PINV[i, j] = (double)LD[i * interval, j];     //这是一个采样的过程
                        }
                        PD[i] = (double)D[i * interval, 0];
                    }
                    double[] input = new double[900];
                    for (int i = 0; i < 100; i++)
                    {
                        for (int j = 0; j < 9; j++)
                        {
                            input[i * 9 + j] = PINV[i, j];
                        }
                    }
                    double* mydata = stackalloc double[900];
                    double** result;
                    for (int i = 0; i < 900; i++)
                        mydata[i] = input[i];
                    result = mycal(100, 9, mydata, 0.001);
                    double[,] ANSWER = new double[9, 100];
                    for (int i = 0; i < 9; i++)
                        for (int j = 0; j < 100; j++)
                            ANSWER[i, j] = result[i][j];
                    //到此处，广义逆解已经求出,存于ANSWER;
                    double[] transform = new double[9];
                    for (int i = 0; i < 9; i++)
                    {
                        transform[i] = 0;
                        for (int j = 0; j < 100; j++)
                        {
                            transform[i] += ANSWER[i, j] * PD[j];
                        }
                    }
                    Matrix<double> Q = myorth(transform);
                    for (int i = 0; i < 3; i++)
                    {
                        str = "";
                        for (int j = 0; j < 4; j++)
                            str = str + Q[i, j].ToString() + "  ";
                        richTextBox_Show.SelectionColor = Color.Red;
                        richTextBox_Show.AppendText(str);
                        richTextBox_Show.ScrollToCaret();
                    }

                    textBox_Rotation11.Text = Q[0, 0].ToString("0.0000");
                    textBox_Rotation12.Text = Q[0, 1].ToString("0.0000");
                    textBox_Rotation13.Text = Q[0, 2].ToString("0.0000");
                    textBox_Rotation21.Text = Q[1, 0].ToString("0.0000");
                    textBox_Rotation22.Text = Q[1, 1].ToString("0.0000");
                    textBox_Rotation23.Text = Q[1, 2].ToString("0.0000");
                    textBox_Rotation31.Text = Q[2, 0].ToString("0.0000");
                    textBox_Rotation32.Text = Q[2, 1].ToString("0.0000");
                    textBox_Rotation33.Text = Q[2, 2].ToString("0.0000");
                    textBox_DeviationX.Text = Q[0, 3].ToString("0.0000");
                    textBox_DeviationY.Text = Q[1, 3].ToString("0.0000");
                    textBox_DeviationZ.Text = Q[2, 3].ToString("0.0000");
                    //此时读完A值
                    richTextBox_Show.SelectionColor = Color.Black;
                    richTextBox_Show.AppendText("完成！ \n");
                    richTextBox_Show.ScrollToCaret();
                }
            }
            catch (System.Exception ex)
            {
                richTextBox_Show.SelectionColor = Color.Red;
                richTextBox_Show.AppendText(ex.ToString() + '\n');
                richTextBox_Show.AppendText("计算外部参数失败！");
                richTextBox_Show.ScrollToCaret();
            }
          
        }

        private Matrix<double> myorth(double[] input)
        {
            Matrix<double> H = new Matrix<double>(3, 3);
            for (int i = 0; i < 3; i++)
            {
                for (int j = 0; j < 3; j++)
                {
                    H[i, j] = input[3 * i + j];
                }
            }
            Matrix<double> W = new Matrix<double>(3, 3);
            for (int i = 0; i < 3; i++)
            {
                W[i, 0] = H[i, 0];
            }
            Matrix<double> data_tem = new Matrix<double>(3, 1);
            data_tem[0, 0] = H[1, 0] * H[2, 1] - H[2, 0] * H[1, 1];
            data_tem[1, 0] = H[2, 0] * H[0, 1] - H[0, 0] * H[2, 1];
            data_tem[2, 0] = H[0, 0] * H[1, 1] - H[1, 0] * H[0, 1];
            for (int i = 0; i < 3; i++)
            {
                W[i, 1] = -data_tem[i, 0];
                W[i, 2] = H[i, 1];
            }
            Matrix<double> col2 = new Matrix<double>(3, 1);
            for (int i = 0; i < 3; i++)
                col2[i, 0] = -H[i, 2];
            Matrix<double> WW = W.Transpose();
            Matrix<double> V = WW * col2;
            double[] orthin = new double[9];
            double[] orthout = new double[9];
            for (int i = 0; i < 3; i++)
                for (int j = 0; j < 3; j++)
                    orthin[3 * i + j] = WW[i, j];
            matorth(orthin, orthout);
            for (int i = 0; i < 3; i++)
                for (int j = 0; j < 3; j++)
                    WW[i, j] = orthout[3 * i + j];
            Matrix<double> P = new Matrix<double>(3, 4);
            for (int i = 0; i < 3; i++)
                for (int j = 0; j < 3; j++)
                    P[i, j] = WW[i, j];
            for (int i = 0; i < 3; i++)
                P[i, 3] = V[i, 0];
            return P;
        }

        [DllImport("mydll.dll", CallingConvention = CallingConvention.Cdecl)]
        static extern unsafe double** mycal(int m, int n, double* data, double wucha);//求解广义逆

        [DllImport("mydll.dll", CallingConvention = CallingConvention.Cdecl)]
        static extern unsafe void matorth(double[] A, double[] P);  //正交化函数，A.P为长度为9的浮点数序列。

        public void PreClosing()
        {
            if (capture_start)
            {
                _Capture.Stop();
                _Capture.Dispose();
            }
            if (file_init)
            {
                write_LMS111rawdata.Close();
                write_LMS111valuable.Close();
                write_EX_Param.Close();
                write_IC_Param.Close();
            }
            if (LMS111_serial.IsOpen)
            {
                LMS111_serial.Close();
                LMS111_serial.DataReceived -= new SerialDataReceivedEventHandler(event_LMS111ReadSerialport);
            }
        }

        // Ensures the disposing of the capture device
        protected override void OnFormClosing(FormClosingEventArgs e)
        {
            PreClosing();

            base.OnFormClosing(e);   //在MSDN中规定如果重载了OnFormClosing函数，最后要调用基类的OnFormClosing函数
        }
        #endregion

        #region 触发中断
        //处理抓取的图像
        private void _Capture_ImageGrabbed(object sender, EventArgs e)
        {
            try
            {
                //lets get a frame from our capture device
                img = _Capture.RetrieveBgrFrame();
                Gray_Frame = img.Convert<Gray, Byte>();

                if (currentMode == Mode.Preview)
                {
                    if (flag_previewfirst)
                    {
                        ReadCaliParameters();
                        SendtoLMS111_single();
                        flag_previewfirst = false;
                    }

                    Matrix<float> Map1, Map2;
                    IC.InitUndistortMap(img.Width, img.Height, out Map1, out Map2);

                    //remap the image to the particular intrinsics
                    //In the current version of EMGU any pixel that is not corrected is set to transparent allowing the original image to be displayed if the same
                    //image is mapped backed, in the future this should be controllable through the flag '0'
                    Image<Bgr, Byte> temp = img.CopyBlank();
                    CvInvoke.cvRemap(img, temp, Map1, Map2, 0, new MCvScalar(0));
                    img = temp.Copy();

                    //绘制激光点
                    if (LMS111_open_cali && flag_preview)
                    {
                        if (flag_lmsdata_done)
                        {
                            for (int i = 0; i < LMS_NUM_cali; ++i)
                            {
                                LMS_u_temp[i] = LMS_u[i];
                                LMS_v_temp[i] = LMS_v[i];
                            }
                            SendtoLMS111_single();
                            flag_lmsdata_done = false;
                        }

                        //画点
                        for (int i = 0; i < LMS_NUM_cali; i++)
                            if (LMS_u_temp[i] < 640 && LMS_u_temp[i] > 0 && LMS_v_temp[i] < 480 && LMS_v_temp[i] > 0)
                                img.Draw(new CircleF(new Point(LMS_u_temp[i], LMS_v_temp[i]), 3), new Bgr(Color.Blue), 3);
                    }
                }
                else if (para_sure)
                {
                    //apply chess board detection
                    if (currentMode == Mode.SavingFrames)
                    {
                        if (start_Flag)
                        {
                            textBox_CalibrationState.Invoke(new EventHandler(delegate { textBox_CalibrationState.Text = "存储角点图像..."; }));
                        }
                        else
                        {
                            textBox_CalibrationState.Invoke(new EventHandler(delegate { textBox_CalibrationState.Text = "等待运行..."; }));
                        }

                        corners = CameraCalibration.FindChessboardCorners(Gray_Frame, patternSize, Emgu.CV.CvEnum.CALIB_CB_TYPE.ADAPTIVE_THRESH);
                        //we use this loop so we can show a colour image rather than a gray: //CameraCalibration.DrawChessboardCorners(Gray_Frame, patternSize, corners);

                        if (corners != null) //chess board found
                        {
                            //make mesurments more accurate by using FindCornerSubPixel
                            Gray_Frame.FindCornerSubPix(new PointF[1][] { corners }, new Size(11, 11), new Size(-1, -1), new MCvTermCriteria(30, 0.1));

                            //if go button has been pressed start aquiring frames else we will just display the points
                            if (start_Flag && manual_flag)
                            {
                                Frame_array_buffer[frame_buffer_savepoint] = Gray_Frame.Copy(); //store the image
                                frame_buffer_savepoint_pre = frame_buffer_savepoint;
                                frame_buffer_savepoint++;//increase buffer positon
                                if (LMS111_open_cali)
                                {
                                    SendtoLMS111_single(); //向LMS111发送指令,获得激光数据
                                    button_munal.Invoke(new EventHandler(delegate { button_munal.Enabled = false; })); //为了保证每次激光数据都截取一次
                                }
                                textBox_ImageDone.Invoke(new EventHandler(delegate { textBox_ImageDone.Text = frame_buffer_savepoint.ToString(); }));
                                if (checkBox_saveImage.Checked == true)//保存原始图像
                                {
                                    if (radioButton_GrayImage.Checked)
                                        Gray_Frame.Save(mainwindows_calibration.path + "/doc/ImageSaved/" + frame_buffer_savepoint.ToString() + ".bmp");
                                    else
                                        img.Save(mainwindows_calibration.path + "/doc/ImageSaved/" + frame_buffer_savepoint.ToString() + ".bmp");
                                }
                                //check the state of buffer
                                if (frame_buffer_savepoint == Frame_array_buffer.Length)
                                    button_munal.Invoke(new EventHandler(delegate { button_munal.Text = "计算"; }));

                                if (radioButton_auto.Checked == false) manual_flag = false;
                            }

                            //draw the results
                            img.Draw(new CircleF(corners[0], 4), new Bgr(Color.Blue), 2);
                            for (int i = 1; i < corners.Length; i++)
                            {
                                img.Draw(new LineSegment2DF(corners[i - 1], corners[i]), line_colour_array[i], 2);
                                img.Draw(new CircleF(corners[i], 4), new Bgr(Color.Blue), 2);
                            }
                            //calibrate the delay bassed on size of buffer
                            //if buffer small you want a big delay if big small delay
                            Thread.Sleep(interval);//allow the user to move the board to a different position
                        }
                        corners = null;
                    }
                    if (currentMode == Mode.Caluculating_Intrinsics)
                    {
                        //we can do this in the loop above to increase speed
                        for (int k = 0; k < Frame_array_buffer.Length; k++)
                        {
                            textBox_CalibrationState.Invoke(new EventHandler(delegate { textBox_CalibrationState.Text = "计算标定参数... 第" + (k + 1).ToString() + "个"; }));
                            corners_points_list[k] = CameraCalibration.FindChessboardCorners(Frame_array_buffer[k], patternSize, Emgu.CV.CvEnum.CALIB_CB_TYPE.ADAPTIVE_THRESH);
                            //for accuracy
                            Frame_array_buffer[k].FindCornerSubPix(corners_points_list, new Size(11, 11), new Size(-1, -1), new MCvTermCriteria(30, 0.1)); //?????为什么是Gray_Frame

                            //Fill our objects list with the real world mesurments for the intrinsic calculations
                            List<MCvPoint3D32f> object_list = new List<MCvPoint3D32f>();
                            for (int i = 0; i < height; i++)
                            {
                                for (int j = 0; j < width; j++)
                                {
                                    object_list.Add(new MCvPoint3D32f(j * Convert.ToSingle(textBox_SquareWidth.Text)/1000, i * Convert.ToSingle(textBox_SquareHeight.Text)/1000, 0.0F)); //真实棋盘方格的大小
                                }
                            }
                            corners_object_list[k] = object_list.ToArray();
                        }

                        //our error should be as close to 0 as possible
                        textBox_CalibrationState.Invoke(new EventHandler(delegate { textBox_CalibrationState.Text = "计算标定参数..."; }));
                        double error = CameraCalibration.CalibrateCamera(corners_object_list, corners_points_list, Gray_Frame.Size, IC, Emgu.CV.CvEnum.CALIB_TYPE.CV_CALIB_FIX_K3, out EX_Param);
                        //If Emgu.CV.CvEnum.CALIB_TYPE == CV_CALIB_USE_INTRINSIC_GUESS and/or CV_CALIB_FIX_ASPECT_RATIO are specified, some or all of fx, fy, cx, cy must be initialized before calling the function
                        //if you use FIX_ASPECT_RATIO and FIX_FOCAL_LEGNTH options, these values needs to be set in the intrinsic parameters before the CalibrateCamera function is called. Otherwise 0 values are used as default.
                        MessageBox.Show("Intrinsci Calculation Error: " + error.ToString(), "Results", MessageBoxButtons.OK, MessageBoxIcon.Information); //display the results to the user

                        //显示计算出的参数
                        textBox_Intrinsic11.Invoke(new EventHandler(delegate { textBox_Intrinsic11.Text = IC.IntrinsicMatrix.Data[0, 0].ToString("0.0000"); }));
                        textBox_Intrinsic12.Invoke(new EventHandler(delegate { textBox_Intrinsic12.Text = IC.IntrinsicMatrix.Data[0, 1].ToString("0.0000"); }));
                        textBox_Intrinsic13.Invoke(new EventHandler(delegate { textBox_Intrinsic13.Text = IC.IntrinsicMatrix.Data[0, 2].ToString("0.0000"); }));
                        textBox_Intrinsic21.Invoke(new EventHandler(delegate { textBox_Intrinsic21.Text = IC.IntrinsicMatrix.Data[1, 0].ToString("0.0000"); }));
                        textBox_Intrinsic22.Invoke(new EventHandler(delegate { textBox_Intrinsic22.Text = IC.IntrinsicMatrix.Data[1, 1].ToString("0.0000"); }));
                        textBox_Intrinsic23.Invoke(new EventHandler(delegate { textBox_Intrinsic23.Text = IC.IntrinsicMatrix.Data[1, 2].ToString("0.0000"); }));
                        textBox_Intrinsic31.Invoke(new EventHandler(delegate { textBox_Intrinsic31.Text = IC.IntrinsicMatrix.Data[2, 0].ToString("0.0000"); }));
                        textBox_Intrinsic32.Invoke(new EventHandler(delegate { textBox_Intrinsic32.Text = IC.IntrinsicMatrix.Data[2, 1].ToString("0.0000"); }));
                        textBox_Intrinsic33.Invoke(new EventHandler(delegate { textBox_Intrinsic33.Text = IC.IntrinsicMatrix.Data[2, 2].ToString("0.0000"); }));
                        textBox_DistortionK1.Invoke(new EventHandler(delegate { textBox_DistortionK1.Text = IC.DistortionCoeffs.Data[0, 0].ToString("0.0000"); }));
                        textBox_DistortionK2.Invoke(new EventHandler(delegate { textBox_DistortionK2.Text = IC.DistortionCoeffs.Data[1, 0].ToString("0.0000"); }));
                        textBox_DistortionK3.Invoke(new EventHandler(delegate { textBox_DistortionK3.Text = IC.DistortionCoeffs.Data[4, 0].ToString("0.0000"); }));
                        textBox_DistortionP1.Invoke(new EventHandler(delegate { textBox_DistortionP1.Text = IC.DistortionCoeffs.Data[2, 0].ToString("0.0000"); }));
                        textBox_DistortionP2.Invoke(new EventHandler(delegate { textBox_DistortionP2.Text = IC.DistortionCoeffs.Data[3, 0].ToString("0.0000"); }));

                        //保存相关参数
                        if (file_init) //判断是否已经初始化
                        {
                            write_IC_Param.WriteLine("IntrinsicMatrix");//保存内部矩阵
                            for (int i = 0; i < 3; i++)
                            {
                                for (int j = 0; j < 3; j++)
                                {
                                    write_IC_Param.Write(IC.IntrinsicMatrix.Data[i, j].ToString() + "\t");
                                }
                                write_IC_Param.WriteLine();
                            }
                            write_IC_Param.WriteLine("DistortionCoeffs(K1,K2,P1,P2,K3)");
                            for (int i = 0; i < 5; i++)
                            {
                                write_IC_Param.Write(IC.DistortionCoeffs.Data[i, 0].ToString() + "\t");
                            }

                            write_EX_Param.WriteLine("ExtrinsicParameters " + frame_buffer_savepoint.ToString());
                            for (int i = 0; i < EX_Param.Length; i++)
                            {
                                write_EX_Param.WriteLine();
                                write_EX_Param.WriteLine("[" + i.ToString() + "]");
                                write_EX_Param.WriteLine("Rotation:");
                                for (int m = 0; m < 3; m++)
                                {
                                    for (int n = 0; n < 3; n++)
                                    {
                                        write_EX_Param.Write(EX_Param[i].RotationVector.RotationMatrix.Data[m, n].ToString() + "\t");
                                    }
                                    write_EX_Param.WriteLine();
                                }

                                write_EX_Param.WriteLine("Translation:");
                                for (int m = 0; m < 3; m++)
                                {
                                    write_EX_Param.Write(EX_Param[i].TranslationVector.Data[m, 0].ToString() + "\t");
                                }
                                write_EX_Param.WriteLine();
                            }
                            checkBox_HistoryData.Invoke(new EventHandler(delegate { checkBox_HistoryData.Enabled = true; })); //可以使用复选框
                        }

                        currentMode = Mode.Calibrated;
                    }
                    if (currentMode == Mode.Calibrated)
                    {
                        textBox_CalibrationState.Invoke(new EventHandler(delegate { textBox_CalibrationState.Text = "标定完成！"; }));
                        //display the original image
                        Sub_PicturBox.Image = img.ToBitmap();
                        //calculate the camera intrinsics
                        Matrix<float> Map1, Map2;
                        IC.InitUndistortMap(img.Width, img.Height, out Map1, out Map2);

                        //remap the image to the particular intrinsics
                        //In the current version of EMGU any pixel that is not corrected is set to transparent allowing the original image to be displayed if the same
                        //image is mapped backed, in the future this should be controllable through the flag '0'
                        Image<Bgr, Byte> temp = img.CopyBlank();
                        CvInvoke.cvRemap(img, temp, Map1, Map2, 0, new MCvScalar(0));
                        img = temp.Copy();

                        start_Flag = false;
                    }
                }
                Main_Picturebox.Image = img.Convert<Bgr, Byte>();
            }
            catch (System.Exception)
            {

            }
        }

        private void event_LMS111ReadSerialport(object sender, SerialDataReceivedEventArgs e)
        {
            int data1 = 0;
            byte data = 0;
            byte data_temp = 0;
            //LMS111变量
            int count1 = 0;
            int count2 = 0;
            int count3 = 0;
            bool flag_start_lms = false;
            bool flag_data = false;
            byte[] temp = new byte[8];

            if (serial_receive_on)
            {
            ReadData_LMS: data1 = LMS111_serial.ReadByte();
                data = (byte)data1;

                if (data == 0x02)
                {
                    flag_data = true;
                }
                if (flag_data)
                {
                    if (data1 == 0x03)
                    {
                        switch (lmsState)
                        {
                            case LMSdataState.Idle:
                                {

                                    break;
                                }
                            case LMSdataState.UnionCali:
                                {
                                    //显示
                                    richTextBox_Show.Invoke(new EventHandler(delegate
                                    {
                                        richTextBox_Show.Clear();  //在显示一组激光数据点前清空
                                        richTextBox_Show.SelectionColor = Color.Red;
                                        richTextBox_Show.AppendText("第" + frame_buffer_savepoint.ToString() + "幅图像\n");
                                    }));
                                    
                                    for (int i = 0; i < LMS_NUM_cali; i++)
                                    {
                                        richTextBox_Show.Invoke(new EventHandler(delegate
                                        {
                                            //richbox中的显示
                                            richTextBox_Show.SelectionColor = Color.Black;
                                            richTextBox_Show.AppendText("[" + i.ToString() + "] ");
                                            richTextBox_Show.SelectionColor = Color.Blue;
                                            richTextBox_Show.AppendText(LMS_data_cali[i].ToString());
                                            richTextBox_Show.SelectionColor = Color.Purple;
                                            richTextBox_Show.AppendText(" mm\n");
                                            richTextBox_Show.ScrollToCaret();
                                        }));
                                    }
                                    richTextBox_Show.Invoke(new EventHandler(delegate
                                    {
                                        richTextBox_Show.SelectionColor = Color.Blue;
                                        richTextBox_Show.AppendText("请截取有效数据...\n");
                                        richTextBox_Show.ScrollToCaret();
                                    }));
                                    LMS111_dataReceived = true; //用于激光数据示意图
                                    break;
                                }

                            case LMSdataState.LMSCali:
                                {
                                    richTextBox_Show.BeginInvoke(new EventHandler(delegate
                                    {
                                        for (int i = 0; i < LMS_NUM_cali; i++)
                                        {
                                            //richbox中的显示
                                            richTextBox_Show.SelectionColor = Color.Black;
                                            richTextBox_Show.AppendText("[" + i.ToString() + "] ");
                                            richTextBox_Show.SelectionColor = Color.Blue;
                                            richTextBox_Show.AppendText(LMS_data_cali[i].ToString());
                                            richTextBox_Show.SelectionColor = Color.Purple;
                                            richTextBox_Show.AppendText(" mm\n");
                                            richTextBox_Show.ScrollToCaret();
                                        }
                                    }));

                                    if (flag_alpha1)
                                    {
                                        for (int i = 0; i < LMS_NUM_cali; i++)
                                        {
                                            double angle = (LMS_StartAngle + i * LMS_Resolution) * Math.PI / 180;
                                            double dis = (double)LMS_data_cali[i];
                                            LMS_data_alpha1[i] = dis;
                                            LMS_data_alpha1X[i] = dis * Math.Cos(angle);
                                            LMS_data_alpha1Y[i] = dis * Math.Sin(angle);
                                        }

                                        chart_LMS111data.BeginInvoke(new EventHandler(delegate
                                        {
                                            chart_LMS111data.Series[0].Points.Clear();
                                            chart_LMS111data.Series[1].Points.Clear();

                                            double Ymax = Math.Max(LMS_data_alpha1Y.Max(), LMS_data_alpha2Y.Max());
                                            double Xwide = Math.Max(Math.Max(Math.Abs(LMS_data_alpha1X.Min()), Math.Abs(LMS_data_alpha1X.Max())),
                                                Math.Max(Math.Abs(LMS_data_alpha2X.Min()), Math.Abs(LMS_data_alpha2X.Max())));
                                            chart_LMS111data.ChartAreas[0].AxisX.Maximum = Xwide * 1.1;
                                            chart_LMS111data.ChartAreas[0].AxisX.Minimum = -Xwide * 1.1;
                                            chart_LMS111data.ChartAreas[0].AxisY.Maximum = Ymax * 1.1;
                                            chart_LMS111data.ChartAreas[0].AxisY.Minimum = -10;

                                            for (int i = 0; i < LMS_NUM_cali; ++i)
                                            {
                                                chart_LMS111data.Series[0].Points.AddXY(LMS_data_alpha1X[i], LMS_data_alpha1Y[i]);
                                            }
                                        }));
                                        
                                        flag_alpha1 = false;
                                    }

                                    if (flag_alpha2)
                                    {
                                        for (int i = 0; i < LMS_NUM_cali; i++)
                                        {
                                            double angle = (LMS_StartAngle + i * LMS_Resolution) * Math.PI / 180;
                                            double dis = (double)LMS_data_cali[i];
                                            LMS_data_alpha2[i] = dis;
                                            LMS_data_alpha2X[i] = dis * Math.Cos(angle);
                                            LMS_data_alpha2Y[i] = dis * Math.Sin(angle);
                                        }

                                        chart_LMS111data.BeginInvoke(new EventHandler(delegate
                                        {
                                            chart_LMS111data.Series[2].Points.Clear();
                                            chart_LMS111data.Series[3].Points.Clear();

                                            double Ymax = Math.Max(LMS_data_alpha1Y.Max(), LMS_data_alpha2Y.Max());
                                            double Xwide = Math.Max(Math.Max(Math.Abs(LMS_data_alpha1X.Min()), Math.Abs(LMS_data_alpha1X.Max())),
                                                Math.Max(Math.Abs(LMS_data_alpha2X.Min()), Math.Abs(LMS_data_alpha2X.Max())));
                                            chart_LMS111data.ChartAreas[0].AxisX.Maximum = Xwide * 1.1;
                                            chart_LMS111data.ChartAreas[0].AxisX.Minimum = -Xwide * 1.1;
                                            chart_LMS111data.ChartAreas[0].AxisY.Maximum = Ymax * 1.1;
                                            chart_LMS111data.ChartAreas[0].AxisY.Minimum = -10;

                                            for (int i = 0; i < LMS_NUM_cali; ++i)
                                            {
                                                chart_LMS111data.Series[2].Points.AddXY(LMS_data_alpha2X[i], LMS_data_alpha2Y[i]);
                                            }
                                        }));

                                        flag_alpha2 = false;
                                    }
                                    break;
                                }

                            case LMSdataState.Preview:
                                {
                                    LMSdateTransform();
                                    flag_lmsdata_done = true;
                                    break;
                                }

                            default:
                                break;
                        }  
                    }
                    else
                    {
                        if ((data == 0x20) && (flag_start_lms == false))
                        {
                            count1++;
                        }
                        if (count1 >= 26)
                        {
                            flag_start_lms = true;
                        }

                        if (count3 == 239)
                        {
                            count3 = 239;
                        }

                        if ((count3 < LMS_NUM_cali) && flag_start_lms)
                        {
                            if (data == 0x20)
                            {
                                switch (count2)
                                {
                                    case 1:
                                        LMS_data_cali[count3] = temp[0];
                                        count3++;
                                        count2 = 0;
                                        break;

                                    case 2:
                                        LMS_data_cali[count3] = (temp[0] << 4) + temp[1];
                                        count3++;
                                        count2 = 0;
                                        break;

                                    case 3:
                                        LMS_data_cali[count3] = (temp[0] << 8) + (temp[1] << 4) + temp[2];
                                        count3++;
                                        count2 = 0;
                                        break;

                                    case 4:
                                        LMS_data_cali[count3] = (temp[0] << 12) + (temp[1] << 8) + (temp[2] << 4) + temp[3];
                                        count3++;
                                        count2 = 0;
                                        break;

                                    case 5:
                                        LMS_data_cali[count3] = (temp[0] << 16) + (temp[1] << 12) + (temp[2] << 8) + (temp[3] << 4) + temp[4];
                                        count3++;
                                        count2 = 0;
                                        break;

                                    case 6:
                                        LMS_data_cali[count3] = (temp[0] << 20) + (temp[1] << 16) + (temp[2] << 12) + (temp[3] << 8) + (temp[4] << 4) + temp[5];
                                        count3++;
                                        count2 = 0;
                                        break;

                                    default:

                                        break;
                                }
                            }
                            else
                            {
                                if ((data >= 0x30) && (data <= 0x39))
                                {
                                    temp[count2] = (byte)(data - 0x30);
                                }
                                else if ((data >= 0x41) && (data <= 0x46))
                                {
                                    temp[count2] = (byte)(data - 0x37);
                                }
                                count2++;
                            }
                        }
                        data_temp = data;
                        goto ReadData_LMS;
                    }
                }
                else
                {
                    goto ReadData_LMS;
                }
            }

        }
        #endregion

        #region 文本框格式检查
        private void textBox_Intrinsic11_TextChanged(object sender, EventArgs e)
        {
            try
            {
                TestText();
                toolStripButton_SaveParameters.Enabled = true;
            }
            catch (FormatException)
            {
                toolStripButton_SaveParameters.Enabled = false;
            }
        }

        private void textBox_Intrinsic12_TextChanged(object sender, EventArgs e)
        {
            try
            {
                TestText();
                toolStripButton_SaveParameters.Enabled = true;
            }
            catch (FormatException)
            {
                toolStripButton_SaveParameters.Enabled = false;
            }
        }

        private void textBox_Intrinsic13_TextChanged(object sender, EventArgs e)
        {
            try
            {
                TestText();
                toolStripButton_SaveParameters.Enabled = true;
            }
            catch (FormatException)
            {
                toolStripButton_SaveParameters.Enabled = false;
            }
        }

        private void textBox_Intrinsic21_TextChanged(object sender, EventArgs e)
        {
            try
            {
                TestText();
                toolStripButton_SaveParameters.Enabled = true;
            }
            catch (FormatException)
            {
                toolStripButton_SaveParameters.Enabled = false;
            }
        }

        private void textBox_Intrinsic22_TextChanged(object sender, EventArgs e)
        {
            try
            {
                TestText();
                toolStripButton_SaveParameters.Enabled = true;
            }
            catch (FormatException)
            {
                toolStripButton_SaveParameters.Enabled = false;
            }
        }

        private void textBox_Intrinsic23_TextChanged(object sender, EventArgs e)
        {
            try
            {
                TestText();
                toolStripButton_SaveParameters.Enabled = true;
            }
            catch (FormatException)
            {
                toolStripButton_SaveParameters.Enabled = false;
            }
        }

        private void textBox_Intrinsic31_TextChanged(object sender, EventArgs e)
        {
            try
            {
                TestText();
                toolStripButton_SaveParameters.Enabled = true;
            }
            catch (FormatException)
            {
                toolStripButton_SaveParameters.Enabled = false;
            }
        }

        private void textBox_Intrinsic32_TextChanged(object sender, EventArgs e)
        {
            try
            {
                TestText();
                toolStripButton_SaveParameters.Enabled = true;
            }
            catch (FormatException)
            {
                toolStripButton_SaveParameters.Enabled = false;
            }
        }

        private void textBox_Intrinsic33_TextChanged(object sender, EventArgs e)
        {
            try
            {
                TestText();
                toolStripButton_SaveParameters.Enabled = true;
            }
            catch (FormatException)
            {
                toolStripButton_SaveParameters.Enabled = false;
            }
        }

        private void textBox_DistortionK1_TextChanged(object sender, EventArgs e)
        {
            try
            {
                TestText();
                toolStripButton_SaveParameters.Enabled = true;
            }
            catch (FormatException)
            {
                toolStripButton_SaveParameters.Enabled = false;
            }
        }

        private void textBox_DistortionK2_TextChanged(object sender, EventArgs e)
        {
            try
            {
                TestText();
                toolStripButton_SaveParameters.Enabled = true;
            }
            catch (FormatException)
            {
                toolStripButton_SaveParameters.Enabled = false;
            }
        }

        private void textBox_DistortionK3_TextChanged(object sender, EventArgs e)
        {
            try
            {
                TestText();
                toolStripButton_SaveParameters.Enabled = true;
            }
            catch (FormatException)
            {
                toolStripButton_SaveParameters.Enabled = false;
            }
        }

        private void textBox_DistortionP1_TextChanged(object sender, EventArgs e)
        {
            try
            {
                TestText();
                toolStripButton_SaveParameters.Enabled = true;
            }
            catch (FormatException)
            {
                toolStripButton_SaveParameters.Enabled = false;
            }
        }

        private void textBox_DistortionP2_TextChanged(object sender, EventArgs e)
        {
            try
            {
                TestText();
                toolStripButton_SaveParameters.Enabled = true;
            }
            catch (FormatException)
            {
                toolStripButton_SaveParameters.Enabled = false;
            }
        }

        private void textBox_alpha_TextChanged(object sender, EventArgs e)
        {
            try
            {
                TestText();
                toolStripButton_SaveParameters.Enabled = true;
            }
            catch (FormatException)
            {
                toolStripButton_SaveParameters.Enabled = false;
            }
        }

        private void textBox_Rotation11_TextChanged(object sender, EventArgs e)
        {
            try
            {
                TestText();
                toolStripButton_SaveParameters.Enabled = true;
            }
            catch (FormatException)
            {
                toolStripButton_SaveParameters.Enabled = false;
            }
        }

        private void textBox_Rotation12_TextChanged(object sender, EventArgs e)
        {
            try
            {
                TestText();
                toolStripButton_SaveParameters.Enabled = true;
            }
            catch (FormatException)
            {
                toolStripButton_SaveParameters.Enabled = false;
            }
        }

        private void textBox_Rotation13_TextChanged(object sender, EventArgs e)
        {
            try
            {
                TestText();
                toolStripButton_SaveParameters.Enabled = true;
            }
            catch (FormatException)
            {
                toolStripButton_SaveParameters.Enabled = false;
            }
        }

        private void textBox_Rotation21_TextChanged(object sender, EventArgs e)
        {
            try
            {
                TestText();
                toolStripButton_SaveParameters.Enabled = true;
            }
            catch (FormatException)
            {
                toolStripButton_SaveParameters.Enabled = false;
            }
        }

        private void textBox_Rotation22_TextChanged(object sender, EventArgs e)
        {
            try
            {
                TestText();
                toolStripButton_SaveParameters.Enabled = true;
            }
            catch (FormatException)
            {
                toolStripButton_SaveParameters.Enabled = false;
            }
        }

        private void textBox_Rotation23_TextChanged(object sender, EventArgs e)
        {
            try
            {
                TestText();
                toolStripButton_SaveParameters.Enabled = true;
            }
            catch (FormatException)
            {
                toolStripButton_SaveParameters.Enabled = false;
            }
        }

        private void textBox_Rotation31_TextChanged(object sender, EventArgs e)
        {
            try
            {
                TestText();
                toolStripButton_SaveParameters.Enabled = true;
            }
            catch (FormatException)
            {
                toolStripButton_SaveParameters.Enabled = false;
            }
        }

        private void textBox_Rotation32_TextChanged(object sender, EventArgs e)
        {
            try
            {
                TestText();
                toolStripButton_SaveParameters.Enabled = true;
            }
            catch (FormatException)
            {
                toolStripButton_SaveParameters.Enabled = false;
            }
        }

        private void textBox_Rotation33_TextChanged(object sender, EventArgs e)
        {
            try
            {
                TestText();
                toolStripButton_SaveParameters.Enabled = true;
            }
            catch (FormatException)
            {
                toolStripButton_SaveParameters.Enabled = false;
            }
        }

        private void textBox_DeviationX_TextChanged(object sender, EventArgs e)
        {
            try
            {
                TestText();
                toolStripButton_SaveParameters.Enabled = true;
            }
            catch (FormatException)
            {
                toolStripButton_SaveParameters.Enabled = false;
            }
        }

        private void textBox_DeviationY_TextChanged(object sender, EventArgs e)
        {
            try
            {
                TestText();
                toolStripButton_SaveParameters.Enabled = true;
            }
            catch (FormatException)
            {
                toolStripButton_SaveParameters.Enabled = false;
            }
        }

        private void textBox_DeviationZ_TextChanged(object sender, EventArgs e)
        {
            try
            {
                TestText();
                toolStripButton_SaveParameters.Enabled = true;
            }
            catch (FormatException)
            {
                toolStripButton_SaveParameters.Enabled = false;
            }
        }

        private void textBox_XAdjust_TextChanged(object sender, EventArgs e)
        {
            try
            {
                double test;
                test = Convert.ToDouble(textBox_XAdjust.Text);
                test = Convert.ToDouble(textBox_YAdjust.Text);
                test = Convert.ToDouble(textBox_ZAdjust.Text);

                button_Transform.Enabled = true;
            }
            catch (System.FormatException)
            {
                button_Transform.Enabled = false;
            }
        }

        private void textBox_YAdjust_TextChanged(object sender, EventArgs e)
        {
            try
            {
                double test;
                test = Convert.ToDouble(textBox_XAdjust.Text);
                test = Convert.ToDouble(textBox_YAdjust.Text);
                test = Convert.ToDouble(textBox_ZAdjust.Text);

                button_Transform.Enabled = true;
            }
            catch (System.FormatException)
            {
                button_Transform.Enabled = false;
            }
        }

        private void textBox_ZAdjust_TextChanged(object sender, EventArgs e)
        {
            try
            {
                double test;
                test = Convert.ToDouble(textBox_XAdjust.Text);
                test = Convert.ToDouble(textBox_YAdjust.Text);
                test = Convert.ToDouble(textBox_ZAdjust.Text);

                button_Transform.Enabled = true;
            }
            catch (System.FormatException)
            {
                button_Transform.Enabled = false;
            }
        }

        private void textBox_SquareWidth_TextChanged(object sender, EventArgs e)
        {
            try
            {
                double test;
                test = Convert.ToDouble(textBox_SquareHeight.Text);
                test = Convert.ToDouble(textBox_SquareWidth.Text);
                if (capture_flag)
                    button_ParameterSure.Enabled = true;
                else
                    button_ParameterSure.Enabled = false;
            }
            catch (System.FormatException)
            {
                button_ParameterSure.Enabled = false;
            }
        }

        private void textBox_SquareHeight_TextChanged(object sender, EventArgs e)
        {
            try
            {
                double test;
                test = Convert.ToDouble(textBox_SquareHeight.Text);
                test = Convert.ToDouble(textBox_SquareWidth.Text);
                if (capture_flag)
                    button_ParameterSure.Enabled = true;
                else
                    button_ParameterSure.Enabled = false;
            }
            catch (System.FormatException)
            {
                button_ParameterSure.Enabled = false;
            }
        }

        #endregion
    }
}
